diff --git a/MbDCode/ASMTAnimationParameters.cpp b/MbDCode/ASMTAnimationParameters.cpp new file mode 100644 index 0000000..b736926 --- /dev/null +++ b/MbDCode/ASMTAnimationParameters.cpp @@ -0,0 +1,36 @@ +#include "ASMTAnimationParameters.h" + +using namespace MbD; + +void MbD::ASMTAnimationParameters::parseASMT(std::vector& lines) +{ + //int nframe, icurrent, istart, iend, framesPerSecond; + //bool isForward; + size_t pos = lines[0].find_first_not_of("\t"); + auto leadingTabs = lines[0].substr(0, pos); + assert(lines[0] == (leadingTabs + "nframe")); + lines.erase(lines.begin()); + nframe = readInt(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "icurrent")); + lines.erase(lines.begin()); + icurrent = readInt(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "istart")); + lines.erase(lines.begin()); + istart = readInt(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "iend")); + lines.erase(lines.begin()); + iend = readInt(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "isForward")); + lines.erase(lines.begin()); + isForward = readBool(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "framesPerSecond")); + lines.erase(lines.begin()); + framesPerSecond = readInt(lines[0]); + lines.erase(lines.begin()); + +} diff --git a/MbDCode/ASMTAnimationParameters.h b/MbDCode/ASMTAnimationParameters.h new file mode 100644 index 0000000..ac32e2c --- /dev/null +++ b/MbDCode/ASMTAnimationParameters.h @@ -0,0 +1,17 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTAnimationParameters : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + int nframe, icurrent, istart, iend, framesPerSecond; + bool isForward; + + }; +} + diff --git a/MbDCode/ASMTAssembly.cpp b/MbDCode/ASMTAssembly.cpp new file mode 100644 index 0000000..304d873 --- /dev/null +++ b/MbDCode/ASMTAssembly.cpp @@ -0,0 +1,291 @@ +#include +#include +#include + +#include "ASMTAssembly.h" +#include "CREATE.h" +#include "ASMTRevoluteJoint.h" +#include "ASMTCylindricalJoint.h" +#include "ASMTRotationalMotion.h" +#include "ASMTTranslationalMotion.h" + +using namespace MbD; + +void MbD::ASMTAssembly::runFile(const char* chars) +{ + std::string str; + std::ifstream in(chars); + std::string line; + std::vector lines; + while (std::getline(in, line)) { + lines.push_back(line); + } + assert(lines[0] == "freeCAD: 3D CAD with Motion Simulation by askoh.com"); + lines.erase(lines.begin()); + + if (lines[0] == "Assembly") { + lines.erase(lines.begin()); + auto assembly = CREATE::With(); + assembly->parseASMT(lines); + } + +} + +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()); + } + else 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 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 (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 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; + } + else { + assert(false); + } + } +} diff --git a/MbDCode/ASMTAssembly.h b/MbDCode/ASMTAssembly.h new file mode 100644 index 0000000..05fe34d --- /dev/null +++ b/MbDCode/ASMTAssembly.h @@ -0,0 +1,46 @@ +#pragma once +#include + +#include "ASMTItem.h" +#include "ASMTRefPoint.h" +#include "ASMTRefCurve.h" +#include "ASMTRefSurface.h" +#include "ASMTPart.h" +#include "ASMTKinematicIJ.h" +#include "ASMTConstraintSet.h" +#include "ASMTForceTorque.h" +#include "ASMTConstantGravity.h" +#include "ASMTSimulationParameters.h" +#include "ASMTAnimationParameters.h" +#include "FullColumn.h" +#include "FullMatrix.h" +#include "ASMTJoint.h" +#include "ASMTMotion.h" + +namespace MbD { + class ASMTAssembly : public ASMTItem + { + // + public: + static void runFile(const char* chars); + void parseASMT(std::vector& lines) override; + + std::string notes, name; + FColDsptr position3D, velocity3D, omega3D; + FMatDsptr rotationMatrix; + std::shared_ptr>> refPoints; + std::shared_ptr>> refCurves; + std::shared_ptr>> refSurfaces; + std::shared_ptr>> parts; + std::shared_ptr>> kinematicIJs; + std::shared_ptr>> constraintSets; + std::shared_ptr>> joints; + std::shared_ptr>> motions; + std::shared_ptr>> forceTorques; + std::shared_ptr constantGravity; + std::shared_ptr simulationParameters; + std::shared_ptr animationParameters; + + }; +} + diff --git a/MbDCode/ASMTConstantGravity.cpp b/MbDCode/ASMTConstantGravity.cpp new file mode 100644 index 0000000..655a0c3 --- /dev/null +++ b/MbDCode/ASMTConstantGravity.cpp @@ -0,0 +1,9 @@ +#include "ASMTConstantGravity.h" + +using namespace MbD; + +void MbD::ASMTConstantGravity::parseASMT(std::vector& lines) +{ + g = readColumnOfDoubles(lines[0]); + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTConstantGravity.h b/MbDCode/ASMTConstantGravity.h new file mode 100644 index 0000000..fc3cbfd --- /dev/null +++ b/MbDCode/ASMTConstantGravity.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTConstantGravity : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + FColDsptr g; + }; +} + diff --git a/MbDCode/ASMTConstraintSet.cpp b/MbDCode/ASMTConstraintSet.cpp new file mode 100644 index 0000000..7d115bc --- /dev/null +++ b/MbDCode/ASMTConstraintSet.cpp @@ -0,0 +1,3 @@ +#include "ASMTConstraintSet.h" + +using namespace MbD; diff --git a/MbDCode/ASMTConstraintSet.h b/MbDCode/ASMTConstraintSet.h new file mode 100644 index 0000000..22b74f8 --- /dev/null +++ b/MbDCode/ASMTConstraintSet.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTConstraintSet : public ASMTItem + { + // + public: + + + }; +} + diff --git a/MbDCode/ASMTCylindricalJoint.cpp b/MbDCode/ASMTCylindricalJoint.cpp new file mode 100644 index 0000000..98a9b31 --- /dev/null +++ b/MbDCode/ASMTCylindricalJoint.cpp @@ -0,0 +1,4 @@ +#include "ASMTCylindricalJoint.h" + +using namespace MbD; + diff --git a/MbDCode/ASMTCylindricalJoint.h b/MbDCode/ASMTCylindricalJoint.h new file mode 100644 index 0000000..00e37e2 --- /dev/null +++ b/MbDCode/ASMTCylindricalJoint.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ASMTJoint.h" + +namespace MbD { + class ASMTCylindricalJoint : public ASMTJoint + { + // + public: + + + }; +} + diff --git a/MbDCode/ASMTExtrusion.cpp b/MbDCode/ASMTExtrusion.cpp new file mode 100644 index 0000000..f81fafd --- /dev/null +++ b/MbDCode/ASMTExtrusion.cpp @@ -0,0 +1,3 @@ +#include "ASMTExtrusion.h" + +using namespace MbD; diff --git a/MbDCode/ASMTExtrusion.h b/MbDCode/ASMTExtrusion.h new file mode 100644 index 0000000..5a89174 --- /dev/null +++ b/MbDCode/ASMTExtrusion.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTExtrusion : public ASMTItem + { + // + public: + + + }; +} + diff --git a/MbDCode/ASMTForceTorque.cpp b/MbDCode/ASMTForceTorque.cpp new file mode 100644 index 0000000..385f360 --- /dev/null +++ b/MbDCode/ASMTForceTorque.cpp @@ -0,0 +1,3 @@ +#include "ASMTForceTorque.h" + +using namespace MbD; diff --git a/MbDCode/ASMTForceTorque.h b/MbDCode/ASMTForceTorque.h new file mode 100644 index 0000000..d4dead2 --- /dev/null +++ b/MbDCode/ASMTForceTorque.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTForceTorque : public ASMTItem + { + // + public: + + + }; +} + diff --git a/MbDCode/ASMTGeneralMotion.cpp b/MbDCode/ASMTGeneralMotion.cpp new file mode 100644 index 0000000..48b279b --- /dev/null +++ b/MbDCode/ASMTGeneralMotion.cpp @@ -0,0 +1,7 @@ +#include "ASMTGeneralMotion.h" + +using namespace MbD; + +void MbD::ASMTGeneralMotion::parseASMT(std::vector& lines) +{ +} diff --git a/MbDCode/ASMTGeneralMotion.h b/MbDCode/ASMTGeneralMotion.h new file mode 100644 index 0000000..054aa33 --- /dev/null +++ b/MbDCode/ASMTGeneralMotion.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ASMTMotion.h" + +namespace MbD { + class ASMTGeneralMotion : public ASMTMotion + { + // + public: + void parseASMT(std::vector& lines) override; + + + }; +} + diff --git a/MbDCode/ASMTItem.cpp b/MbDCode/ASMTItem.cpp new file mode 100644 index 0000000..dcb5908 --- /dev/null +++ b/MbDCode/ASMTItem.cpp @@ -0,0 +1,66 @@ +#include "ASMTItem.h" +#include "CREATE.h" + +using namespace MbD; + +void MbD::ASMTItem::initialize() +{ +} + +void MbD::ASMTItem::parseASMT(std::vector& lines) +{ + assert(false); +} + +FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string line) +{ + std::istringstream iss(line); + auto readRowOfDoubles = std::make_shared>(); + double d; + while (iss >> d) { + readRowOfDoubles->push_back(d); + } + return readRowOfDoubles; +} + +FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string line) +{ + std::istringstream iss(line); + auto readColumnOfDoubles = std::make_shared>(); + double d; + while (iss >> d) { + readColumnOfDoubles->push_back(d); + } + return readColumnOfDoubles; +} + +double MbD::ASMTItem::readDouble(std::string line) +{ + std::istringstream iss(line); + double d; + iss >> d; + return d; +} + +int MbD::ASMTItem::readInt(std::string line) +{ + std::istringstream iss(line); + int i; + iss >> i; + return i; +} + +bool MbD::ASMTItem::readBool(std::string line) +{ + if (line.find("true") != std::string::npos) + { + return true; + } + else if (line.find("false") != std::string::npos) + { + return false; + } + else { + assert(false); + } +} diff --git a/MbDCode/ASMTItem.h b/MbDCode/ASMTItem.h new file mode 100644 index 0000000..2fcbad0 --- /dev/null +++ b/MbDCode/ASMTItem.h @@ -0,0 +1,20 @@ +#pragma once +#include "CREATE.h" + +namespace MbD { + class ASMTItem + { + // + 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); + + ASMTItem* owner; + }; +} + diff --git a/MbDCode/ASMTJoint.cpp b/MbDCode/ASMTJoint.cpp new file mode 100644 index 0000000..316700c --- /dev/null +++ b/MbDCode/ASMTJoint.cpp @@ -0,0 +1,21 @@ +#include "ASMTJoint.h" + +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()); +} diff --git a/MbDCode/ASMTJoint.h b/MbDCode/ASMTJoint.h new file mode 100644 index 0000000..efb1784 --- /dev/null +++ b/MbDCode/ASMTJoint.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ASMTConstraintSet.h" + +namespace MbD { + class ASMTJoint : public ASMTConstraintSet + { + // + public: + void parseASMT(std::vector& lines) override; + + std::string name, markerI, markerJ; + + }; +} diff --git a/MbDCode/ASMTKinematicIJ.cpp b/MbDCode/ASMTKinematicIJ.cpp new file mode 100644 index 0000000..2824700 --- /dev/null +++ b/MbDCode/ASMTKinematicIJ.cpp @@ -0,0 +1,3 @@ +#include "ASMTKinematicIJ.h" + +using namespace MbD; diff --git a/MbDCode/ASMTKinematicIJ.h b/MbDCode/ASMTKinematicIJ.h new file mode 100644 index 0000000..0a86631 --- /dev/null +++ b/MbDCode/ASMTKinematicIJ.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTKinematicIJ : public ASMTItem + { + // + public: + + + }; +} + diff --git a/MbDCode/ASMTMarker.cpp b/MbDCode/ASMTMarker.cpp new file mode 100644 index 0000000..372fc59 --- /dev/null +++ b/MbDCode/ASMTMarker.cpp @@ -0,0 +1,43 @@ +#include "ASMTMarker.h" + +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); + } + } +} diff --git a/MbDCode/ASMTMarker.h b/MbDCode/ASMTMarker.h new file mode 100644 index 0000000..0969a90 --- /dev/null +++ b/MbDCode/ASMTMarker.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ASMTItem.h" +#include "FullColumn.h" +#include "FullMatrix.h" + +namespace MbD { + class ASMTMarker : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + std::string name; + FColDsptr position3D; + FMatDsptr rotationMatrix; + + }; +} + diff --git a/MbDCode/ASMTMotion.cpp b/MbDCode/ASMTMotion.cpp new file mode 100644 index 0000000..c60fa7e --- /dev/null +++ b/MbDCode/ASMTMotion.cpp @@ -0,0 +1,3 @@ +#include "ASMTMotion.h" + +using namespace MbD; diff --git a/MbDCode/ASMTMotion.h b/MbDCode/ASMTMotion.h new file mode 100644 index 0000000..a1c9cd2 --- /dev/null +++ b/MbDCode/ASMTMotion.h @@ -0,0 +1,13 @@ +#pragma once + +#include "ASMTConstraintSet.h" + +namespace MbD { + class ASMTMotion : public ASMTConstraintSet + { + // + public: + + + }; +} diff --git a/MbDCode/ASMTPart.cpp b/MbDCode/ASMTPart.cpp new file mode 100644 index 0000000..ee58a52 --- /dev/null +++ b/MbDCode/ASMTPart.cpp @@ -0,0 +1,127 @@ +#include "ASMTPart.h" +#include "CREATE.h" + +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; + } + } +} diff --git a/MbDCode/ASMTPart.h b/MbDCode/ASMTPart.h new file mode 100644 index 0000000..cab2c96 --- /dev/null +++ b/MbDCode/ASMTPart.h @@ -0,0 +1,27 @@ +#pragma once + +#include "ASMTItem.h" +#include "ASMTRefPoint.h" +#include "ASMTRefCurve.h" +#include "ASMTRefSurface.h" +#include "ASMTPrincipalMassMarker.h" + +namespace MbD { + class ASMTPart : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + 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; + + }; +} + diff --git a/MbDCode/ASMTPrincipalMassMarker.cpp b/MbDCode/ASMTPrincipalMassMarker.cpp new file mode 100644 index 0000000..1a3e7f6 --- /dev/null +++ b/MbDCode/ASMTPrincipalMassMarker.cpp @@ -0,0 +1,45 @@ +#include "ASMTPrincipalMassMarker.h" +#include +#include "FullMatrix.h" + +using namespace MbD; + +void MbD::ASMTPrincipalMassMarker::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 + "Position3D")); + lines.erase(lines.begin()); + position3D = readColumnOfDoubles(lines[0]); + lines.erase(lines.begin()); + assert(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()); + } + assert(lines[0] == (leadingTabs + "Mass")); + lines.erase(lines.begin()); + mass = readDouble(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "MomentOfInertias")); + lines.erase(lines.begin()); + momentOfInertias = std::make_shared>(3); + auto row = readRowOfDoubles(lines[0]); + lines.erase(lines.begin()); + for (int i = 0; i < 3; i++) + { + momentOfInertias->atiput(i, row->at(i)); + } + assert(lines[0] == (leadingTabs + "Density")); + lines.erase(lines.begin()); + density = readDouble(lines[0]); + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTPrincipalMassMarker.h b/MbDCode/ASMTPrincipalMassMarker.h new file mode 100644 index 0000000..7cab1bc --- /dev/null +++ b/MbDCode/ASMTPrincipalMassMarker.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTPrincipalMassMarker : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + std::string name; + FColDsptr position3D; + FMatDsptr rotationMatrix; + double mass, density; + DiagMatDsptr momentOfInertias; + + }; +} + diff --git a/MbDCode/ASMTRefCurve.cpp b/MbDCode/ASMTRefCurve.cpp new file mode 100644 index 0000000..5f41fa2 --- /dev/null +++ b/MbDCode/ASMTRefCurve.cpp @@ -0,0 +1,9 @@ +#include "ASMTRefCurve.h" +#include "CREATE.h" + +using namespace MbD; + +void MbD::ASMTRefCurve::parseASMT(std::vector& lines) +{ + assert(false); +} diff --git a/MbDCode/ASMTRefCurve.h b/MbDCode/ASMTRefCurve.h new file mode 100644 index 0000000..45db368 --- /dev/null +++ b/MbDCode/ASMTRefCurve.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTRefCurve : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + + }; +} + diff --git a/MbDCode/ASMTRefPoint.cpp b/MbDCode/ASMTRefPoint.cpp new file mode 100644 index 0000000..50d74eb --- /dev/null +++ b/MbDCode/ASMTRefPoint.cpp @@ -0,0 +1,58 @@ +#include "ASMTRefPoint.h" +#include "CREATE.h" + +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; + } + } +} diff --git a/MbDCode/ASMTRefPoint.h b/MbDCode/ASMTRefPoint.h new file mode 100644 index 0000000..cdb816f --- /dev/null +++ b/MbDCode/ASMTRefPoint.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ASMTItem.h" +#include +#include +#include "ASMTMarker.h" + +namespace MbD { + class ASMTRefPoint : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + FColDsptr position3D; + FMatDsptr rotationMatrix; + std::shared_ptr>> markers; + }; +} + diff --git a/MbDCode/ASMTRefSurface.cpp b/MbDCode/ASMTRefSurface.cpp new file mode 100644 index 0000000..2263a78 --- /dev/null +++ b/MbDCode/ASMTRefSurface.cpp @@ -0,0 +1,9 @@ +#include "ASMTRefSurface.h" +#include "CREATE.h" + +using namespace MbD; + +void MbD::ASMTRefSurface::parseASMT(std::vector& lines) +{ + assert(false); +} diff --git a/MbDCode/ASMTRefSurface.h b/MbDCode/ASMTRefSurface.h new file mode 100644 index 0000000..0b5085e --- /dev/null +++ b/MbDCode/ASMTRefSurface.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTRefSurface : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + + }; +} + diff --git a/MbDCode/ASMTRevoluteJoint.cpp b/MbDCode/ASMTRevoluteJoint.cpp new file mode 100644 index 0000000..ce18b81 --- /dev/null +++ b/MbDCode/ASMTRevoluteJoint.cpp @@ -0,0 +1,3 @@ +#include "ASMTRevoluteJoint.h" + +using namespace MbD; diff --git a/MbDCode/ASMTRevoluteJoint.h b/MbDCode/ASMTRevoluteJoint.h new file mode 100644 index 0000000..af61164 --- /dev/null +++ b/MbDCode/ASMTRevoluteJoint.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ASMTJoint.h" + +namespace MbD { + class ASMTRevoluteJoint : public ASMTJoint + { + // + public: + + + }; +} + diff --git a/MbDCode/ASMTRotationalMotion.cpp b/MbDCode/ASMTRotationalMotion.cpp new file mode 100644 index 0000000..dc5aef0 --- /dev/null +++ b/MbDCode/ASMTRotationalMotion.cpp @@ -0,0 +1,21 @@ +#include "ASMTRotationalMotion.h" + +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")); + 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]; + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTRotationalMotion.h b/MbDCode/ASMTRotationalMotion.h new file mode 100644 index 0000000..669c9b9 --- /dev/null +++ b/MbDCode/ASMTRotationalMotion.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ASMTMotion.h" + +namespace MbD { + class ASMTRotationalMotion : public ASMTMotion + { + // + public: + void parseASMT(std::vector& lines) override; + + std::string name, motionJoint, rotationZ; + }; +} + diff --git a/MbDCode/ASMTSimulationParameters.cpp b/MbDCode/ASMTSimulationParameters.cpp new file mode 100644 index 0000000..4e4e00f --- /dev/null +++ b/MbDCode/ASMTSimulationParameters.cpp @@ -0,0 +1,36 @@ +#include "ASMTSimulationParameters.h" + +using namespace MbD; + +void MbD::ASMTSimulationParameters::parseASMT(std::vector& lines) +{ + //tstart, tend, hmin, hmax, hout, errorTol; + + size_t pos = lines[0].find_first_not_of("\t"); + auto leadingTabs = lines[0].substr(0, pos); + assert(lines[0] == (leadingTabs + "tstart")); + lines.erase(lines.begin()); + tstart = readDouble(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "tend")); + lines.erase(lines.begin()); + tend = readDouble(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "hmin")); + lines.erase(lines.begin()); + hmin = readDouble(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "hmax")); + lines.erase(lines.begin()); + hmax = readDouble(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "hout")); + lines.erase(lines.begin()); + hout = readDouble(lines[0]); + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "errorTol")); + lines.erase(lines.begin()); + errorTol = readDouble(lines[0]); + lines.erase(lines.begin()); + +} diff --git a/MbDCode/ASMTSimulationParameters.h b/MbDCode/ASMTSimulationParameters.h new file mode 100644 index 0000000..3b6f333 --- /dev/null +++ b/MbDCode/ASMTSimulationParameters.h @@ -0,0 +1,16 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTSimulationParameters : public ASMTItem + { + // + public: + void parseASMT(std::vector& lines) override; + + double tstart, tend, hmin, hmax, hout, errorTol; + + }; +} + diff --git a/MbDCode/ASMTTranslationalMotion.cpp b/MbDCode/ASMTTranslationalMotion.cpp new file mode 100644 index 0000000..e3d6a9b --- /dev/null +++ b/MbDCode/ASMTTranslationalMotion.cpp @@ -0,0 +1,21 @@ +#include "ASMTTranslationalMotion.h" + +using namespace MbD; + +void MbD::ASMTTranslationalMotion::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 + "MotionJoint")); + lines.erase(lines.begin()); + motionJoint = lines[0]; + lines.erase(lines.begin()); + assert(lines[0] == (leadingTabs + "TranslationZ")); + lines.erase(lines.begin()); + translationZ = lines[0]; + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTTranslationalMotion.h b/MbDCode/ASMTTranslationalMotion.h new file mode 100644 index 0000000..38c58c9 --- /dev/null +++ b/MbDCode/ASMTTranslationalMotion.h @@ -0,0 +1,16 @@ +#pragma once + +#include "ASMTMotion.h" + +namespace MbD { + class ASMTTranslationalMotion : public ASMTMotion + { + // + public: + void parseASMT(std::vector& lines) override; + + std::string name, motionJoint, translationZ; + + }; +} + diff --git a/MbDCode/AbsConstraint.h b/MbDCode/AbsConstraint.h index a6febc9..41eb388 100644 --- a/MbDCode/AbsConstraint.h +++ b/MbDCode/AbsConstraint.h @@ -1,4 +1,5 @@ #pragma once + #include "Constraint.h" namespace MbD { class AbsConstraint : public Constraint diff --git a/MbDCode/AngleJoint.cpp b/MbDCode/AngleJoint.cpp index 4d8a7f2..6c9bb55 100644 --- a/MbDCode/AngleJoint.cpp +++ b/MbDCode/AngleJoint.cpp @@ -17,11 +17,9 @@ void MbD::AngleJoint::initializeGlobally() { if (constraints->empty()) { - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2)); - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + auto dirCosIzJz = CREATE::ConstraintWith(frmI, frmJ, 2, 2); + dirCosIzJz->setConstant(std::cos(theIzJz)); + addConstraint(dirCosIzJz); this->root()->hasChanged = true; } else { diff --git a/MbDCode/AngleZIecJec.cpp b/MbDCode/AngleZIecJec.cpp index c0e9c0b..3bb2bf9 100644 --- a/MbDCode/AngleZIecJec.cpp +++ b/MbDCode/AngleZIecJec.cpp @@ -5,6 +5,14 @@ using namespace MbD; +MbD::AngleZIecJec::AngleZIecJec() +{ +} + +MbD::AngleZIecJec::AngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj) : KinematicIeJe(frmi, frmj) +{ +} + void MbD::AngleZIecJec::calcPostDynCorrectorIteration() { auto cthez = aA00IeJe->value(); diff --git a/MbDCode/AngleZIecJec.h b/MbDCode/AngleZIecJec.h index ef1fb5c..294161a 100644 --- a/MbDCode/AngleZIecJec.h +++ b/MbDCode/AngleZIecJec.h @@ -8,6 +8,9 @@ namespace MbD { { //thez aA00IeJe aA10IeJe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq public: + AngleZIecJec(); + AngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj); + void calcPostDynCorrectorIteration() override; virtual void init_aAijIeJe() = 0; void initialize() override; diff --git a/MbDCode/AngleZIeqcJec.cpp b/MbDCode/AngleZIeqcJec.cpp index 08b81e1..609a2ff 100644 --- a/MbDCode/AngleZIeqcJec.cpp +++ b/MbDCode/AngleZIeqcJec.cpp @@ -3,6 +3,14 @@ using namespace MbD; +MbD::AngleZIeqcJec::AngleZIeqcJec() +{ +} + +MbD::AngleZIeqcJec::AngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : AngleZIecJec(frmi, frmj) +{ +} + void MbD::AngleZIeqcJec::calcPostDynCorrectorIteration() { AngleZIecJec::calcPostDynCorrectorIteration(); diff --git a/MbDCode/AngleZIeqcJec.h b/MbDCode/AngleZIeqcJec.h index c84c651..73794c7 100644 --- a/MbDCode/AngleZIeqcJec.h +++ b/MbDCode/AngleZIeqcJec.h @@ -7,6 +7,9 @@ namespace MbD { { //pthezpEI ppthezpEIpEI pcthezpEI psthezpEI public: + AngleZIeqcJec(); + AngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj); + void calcPostDynCorrectorIteration() override; void init_aAijIeJe() override; void initialize() override; diff --git a/MbDCode/AngleZIeqcJeqc.cpp b/MbDCode/AngleZIeqcJeqc.cpp index 65246e4..67bf140 100644 --- a/MbDCode/AngleZIeqcJeqc.cpp +++ b/MbDCode/AngleZIeqcJeqc.cpp @@ -4,6 +4,14 @@ using namespace MbD; +MbD::AngleZIeqcJeqc::AngleZIeqcJeqc() +{ +} + +MbD::AngleZIeqcJeqc::AngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj) : AngleZIeqcJec(frmi, frmj) +{ +} + void MbD::AngleZIeqcJeqc::calcPostDynCorrectorIteration() { AngleZIeqcJec::calcPostDynCorrectorIteration(); diff --git a/MbDCode/AngleZIeqcJeqc.h b/MbDCode/AngleZIeqcJeqc.h index 4f327b3..3fb5b48 100644 --- a/MbDCode/AngleZIeqcJeqc.h +++ b/MbDCode/AngleZIeqcJeqc.h @@ -7,6 +7,9 @@ namespace MbD { { //pthezpEJ ppthezpEIpEJ ppthezpEJpEJ public: + AngleZIeqcJeqc(); + AngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj); + void calcPostDynCorrectorIteration() override; void init_aAijIeJe() override; void initialize() override; diff --git a/MbDCode/Array.h b/MbDCode/Array.h index e7547ca..4cf7126 100644 --- a/MbDCode/Array.h +++ b/MbDCode/Array.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/AtPointJoint.cpp b/MbDCode/AtPointJoint.cpp new file mode 100644 index 0000000..fce65fb --- /dev/null +++ b/MbDCode/AtPointJoint.cpp @@ -0,0 +1,20 @@ +#include "AtPointJoint.h" +#include "System.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::AtPointJoint::AtPointJoint() +{ +} + +MbD::AtPointJoint::AtPointJoint(const char* str) : Joint(str) +{ +} + +void MbD::AtPointJoint::createAtPointConstraints() +{ + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2)); +} diff --git a/MbDCode/AtPointJoint.h b/MbDCode/AtPointJoint.h new file mode 100644 index 0000000..5b7ace8 --- /dev/null +++ b/MbDCode/AtPointJoint.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class AtPointJoint : public Joint + { + // + public: + AtPointJoint(); + AtPointJoint(const char* str); + + void createAtPointConstraints(); + + + }; +} + diff --git a/MbDCode/BasicIntegrator.h b/MbDCode/BasicIntegrator.h index 4d7fcb7..2c06358 100644 --- a/MbDCode/BasicIntegrator.h +++ b/MbDCode/BasicIntegrator.h @@ -1,4 +1,5 @@ #pragma once + #include #include "Integrator.h" diff --git a/MbDCode/BasicUserFunction.h b/MbDCode/BasicUserFunction.h index 17c4b7b..77ba97f 100644 --- a/MbDCode/BasicUserFunction.h +++ b/MbDCode/BasicUserFunction.h @@ -1,4 +1,5 @@ #pragma once + #include #include "UserFunction.h" diff --git a/MbDCode/CADSystem.h b/MbDCode/CADSystem.h index 90618fe..519d9c4 100644 --- a/MbDCode/CADSystem.h +++ b/MbDCode/CADSystem.h @@ -1,4 +1,5 @@ #pragma once + #include #include "System.h" diff --git a/MbDCode/CREATE.h b/MbDCode/CREATE.h index 6ed5f3e..fdf9811 100644 --- a/MbDCode/CREATE.h +++ b/MbDCode/CREATE.h @@ -1,6 +1,7 @@ //This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map. #pragma once + #include #include "EndFrameqct.h" #include "AtPointConstraintIqctJqc.h" diff --git a/MbDCode/CartesianFrame.h b/MbDCode/CartesianFrame.h index 2015ac4..737f742 100644 --- a/MbDCode/CartesianFrame.h +++ b/MbDCode/CartesianFrame.h @@ -1,4 +1,5 @@ #pragma once + #include "Item.h" namespace MbD { diff --git a/MbDCode/ClassDiagram.cd b/MbDCode/ClassDiagram.cd new file mode 100644 index 0000000..7b89419 --- /dev/null +++ b/MbDCode/ClassDiagram.cd @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/MbDCode/CompoundJoint.cpp b/MbDCode/CompoundJoint.cpp new file mode 100644 index 0000000..a3143a6 --- /dev/null +++ b/MbDCode/CompoundJoint.cpp @@ -0,0 +1,11 @@ +#include "CompoundJoint.h" + +using namespace MbD; + +MbD::CompoundJoint::CompoundJoint() +{ +} + +MbD::CompoundJoint::CompoundJoint(const char* str) : Joint(str) +{ +} diff --git a/MbDCode/CompoundJoint.h b/MbDCode/CompoundJoint.h new file mode 100644 index 0000000..cc1a45b --- /dev/null +++ b/MbDCode/CompoundJoint.h @@ -0,0 +1,17 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class CompoundJoint : public Joint + { + //distanceIJ + public: + CompoundJoint(); + CompoundJoint(const char* str); + + double distanceIJ = 0.0; + }; +} + + diff --git a/MbDCode/ConstVelConstraintIJ.cpp b/MbDCode/ConstVelConstraintIJ.cpp index 717cad4..9599ee2 100644 --- a/MbDCode/ConstVelConstraintIJ.cpp +++ b/MbDCode/ConstVelConstraintIJ.cpp @@ -12,6 +12,16 @@ void ConstVelConstraintIJ::calcPostDynCorrectorIteration() aG = aA01IeJe->aAijIeJe + aA10IeJe->aAijIeJe - aConstant; } +void MbD::ConstVelConstraintIJ::initA01IeJe() +{ + assert(false); +} + +void MbD::ConstVelConstraintIJ::initA10IeJe() +{ + assert(false); +} + void ConstVelConstraintIJ::initialize() { this->initA01IeJe(); @@ -37,11 +47,6 @@ void ConstVelConstraintIJ::postInput() Constraint::postInput(); } -ConstraintType ConstVelConstraintIJ::type() -{ - return ConstraintType(); -} - void ConstVelConstraintIJ::postPosICIteration() { aA01IeJe->postPosICIteration(); diff --git a/MbDCode/ConstVelConstraintIJ.h b/MbDCode/ConstVelConstraintIJ.h index 3251e10..4ec9ede 100644 --- a/MbDCode/ConstVelConstraintIJ.h +++ b/MbDCode/ConstVelConstraintIJ.h @@ -12,8 +12,8 @@ namespace MbD { ConstVelConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); void calcPostDynCorrectorIteration() override; - virtual void initA01IeJe() = 0; - virtual void initA10IeJe() = 0; + virtual void initA01IeJe(); + virtual void initA10IeJe(); void initialize() override; void initializeGlobally() override; void initializeLocally() override; @@ -23,7 +23,6 @@ namespace MbD { void prePosIC() override; void preVelIC() override; void simUpdateAll() override; - ConstraintType type() override; std::shared_ptr aA01IeJe, aA10IeJe; }; diff --git a/MbDCode/ConstantVelocityJoint.cpp b/MbDCode/ConstantVelocityJoint.cpp new file mode 100644 index 0000000..7a421c6 --- /dev/null +++ b/MbDCode/ConstantVelocityJoint.cpp @@ -0,0 +1,37 @@ +#include "ConstantVelocityJoint.h" +#include "System.h" +#include "AtPointConstraintIJ.h" +#include "CREATE.h" +#include "ConstVelConstraintIJ.h" + +using namespace MbD; + +MbD::ConstantVelocityJoint::ConstantVelocityJoint() +{ +} + +MbD::ConstantVelocityJoint::ConstantVelocityJoint(const char* str) : AtPointJoint(str) +{ +} + +void MbD::ConstantVelocityJoint::initializeGlobally() +{ + if (constraints->empty()) + { + createAtPointConstraints(); + addConstraint(CREATE::With(frmI, frmJ)); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} + +void MbD::ConstantVelocityJoint::connectsItoJ(EndFrmcptr frmIe, EndFrmcptr frmJe) +{ + //"Subsequent prescribed motions may make frmIe, frmJe become prescribed end frames." + //"Use newCopyEndFrameqc to prevent efrms from becoming EndFrameqct." + + frmI = frmIe->newCopyEndFrameqc(); + frmJ = frmJe->newCopyEndFrameqc(); +} diff --git a/MbDCode/ConstantVelocityJoint.h b/MbDCode/ConstantVelocityJoint.h new file mode 100644 index 0000000..9291ec6 --- /dev/null +++ b/MbDCode/ConstantVelocityJoint.h @@ -0,0 +1,18 @@ +#pragma once + +#include "AtPointJoint.h" + +namespace MbD { + class ConstantVelocityJoint : public AtPointJoint + { + // + public: + ConstantVelocityJoint(); + ConstantVelocityJoint(const char* str); + void initializeGlobally() override; + void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override; + + + }; +} + diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp index dcac26a..0041738 100644 --- a/MbDCode/Constraint.cpp +++ b/MbDCode/Constraint.cpp @@ -105,6 +105,10 @@ void Constraint::removeRedundantConstraints(std::shared_ptr> re assert(false); } +void MbD::Constraint::setConstant(double value) +{ +} + void Constraint::reactivateRedundantConstraints() { //My owner should handle this. diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h index 1910e16..1888a72 100644 --- a/MbDCode/Constraint.h +++ b/MbDCode/Constraint.h @@ -1,4 +1,5 @@ #pragma once + #include #include "enum.h" @@ -33,6 +34,7 @@ namespace MbD { void prePosKine() override; void reactivateRedundantConstraints() override; void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + virtual void setConstant(double value); void setqsudotlam(FColDsptr col) override; void setqsuddotlam(FColDsptr col) override; void setqsulam(FColDsptr col) override; diff --git a/MbDCode/ConstraintIJ.cpp b/MbDCode/ConstraintIJ.cpp index a0261b6..3c95d84 100644 --- a/MbDCode/ConstraintIJ.cpp +++ b/MbDCode/ConstraintIJ.cpp @@ -12,3 +12,8 @@ void ConstraintIJ::initialize() Constraint::initialize(); aConstant = 0.0; } + +void MbD::ConstraintIJ::setConstant(double value) +{ + aConstant = value; +} diff --git a/MbDCode/ConstraintIJ.h b/MbDCode/ConstraintIJ.h index bbf2fa0..587e8a0 100644 --- a/MbDCode/ConstraintIJ.h +++ b/MbDCode/ConstraintIJ.h @@ -11,6 +11,7 @@ namespace MbD { ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); void initialize() override; + void setConstant(double value) override; EndFrmcptr frmI, frmJ; double aConstant; diff --git a/MbDCode/CylSphJoint.cpp b/MbDCode/CylSphJoint.cpp new file mode 100644 index 0000000..725ed28 --- /dev/null +++ b/MbDCode/CylSphJoint.cpp @@ -0,0 +1,28 @@ +#include "CylSphJoint.h" +#include "CREATE.h" +#include "DistancexyConstraintIJ.h" +#include "System.h" + +using namespace MbD; + +MbD::CylSphJoint::CylSphJoint() +{ +} + +MbD::CylSphJoint::CylSphJoint(const char* str) : CompoundJoint(str) +{ +} + +void MbD::CylSphJoint::initializeGlobally() +{ + if (constraints->empty()) + { + auto distxyIJ = CREATE::With(frmI, frmJ); + distxyIJ->setConstant(distanceIJ); + addConstraint(distxyIJ); + this->root()->hasChanged = true; + } + else { + CompoundJoint::initializeGlobally(); + } +} diff --git a/MbDCode/CylSphJoint.h b/MbDCode/CylSphJoint.h new file mode 100644 index 0000000..7742d9f --- /dev/null +++ b/MbDCode/CylSphJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "CompoundJoint.h" + +namespace MbD { + class CylSphJoint : public CompoundJoint + { + // + public: + CylSphJoint(); + CylSphJoint(const char* str); + void initializeGlobally() override; + + + }; +} diff --git a/MbDCode/CylindricalJoint.cpp b/MbDCode/CylindricalJoint.cpp index 3eb317a..3cfe5ed 100644 --- a/MbDCode/CylindricalJoint.cpp +++ b/MbDCode/CylindricalJoint.cpp @@ -6,23 +6,24 @@ using namespace MbD; -CylindricalJoint::CylindricalJoint() { +CylindricalJoint::CylindricalJoint() +{ } -CylindricalJoint::CylindricalJoint(const char* str) : Joint(str) { +CylindricalJoint::CylindricalJoint(const char* str) : InLineJoint(str) +{ } void CylindricalJoint::initializeGlobally() { if (constraints->empty()) { - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); + createInLineConstraints(); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); this->root()->hasChanged = true; } else { - Joint::initializeGlobally(); + InLineJoint::initializeGlobally(); } } diff --git a/MbDCode/CylindricalJoint.h b/MbDCode/CylindricalJoint.h index 02ab376..96c10a4 100644 --- a/MbDCode/CylindricalJoint.h +++ b/MbDCode/CylindricalJoint.h @@ -1,8 +1,9 @@ #pragma once -#include "Joint.h" + +#include "InLineJoint.h" namespace MbD { - class CylindricalJoint : public Joint + class CylindricalJoint : public InLineJoint { //frmI frmJ constraints friction public: diff --git a/MbDCode/DiagonalMatrix.h b/MbDCode/DiagonalMatrix.h index 563e82b..82a7820 100644 --- a/MbDCode/DiagonalMatrix.h +++ b/MbDCode/DiagonalMatrix.h @@ -13,6 +13,7 @@ namespace MbD { DiagonalMatrix(int count) : Array(count) {} DiagonalMatrix(int count, const T& value) : Array(count, value) {} DiagonalMatrix(std::initializer_list list) : Array{ list } {} + void atiput(int i, T value); void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix> diagMat); std::shared_ptr> times(T factor); std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); @@ -30,6 +31,11 @@ namespace MbD { }; template + inline void DiagonalMatrix::atiput(int i, T value) + { + this->at(i) = value; + } + template inline void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr> diagMat) { for (int ii = 0; ii < diagMat->size(); ii++) @@ -108,5 +114,6 @@ namespace MbD { s << "]"; return s; } + using DiagMatDsptr = std::shared_ptr>; } diff --git a/MbDCode/DirectionCosineIecJec.h b/MbDCode/DirectionCosineIecJec.h index 4a5e747..f77bdb0 100644 --- a/MbDCode/DirectionCosineIecJec.h +++ b/MbDCode/DirectionCosineIecJec.h @@ -1,4 +1,5 @@ #pragma once + #include #include "KinematicIeJe.h" diff --git a/MbDCode/DiscontinuityError.h b/MbDCode/DiscontinuityError.h index 19c00d0..a647f85 100644 --- a/MbDCode/DiscontinuityError.h +++ b/MbDCode/DiscontinuityError.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/DispCompIecJecIe.cpp b/MbDCode/DispCompIecJecIe.cpp index 67a2d60..fc55b61 100644 --- a/MbDCode/DispCompIecJecIe.cpp +++ b/MbDCode/DispCompIecJecIe.cpp @@ -9,3 +9,20 @@ MbD::DispCompIecJecIe::DispCompIecJecIe() MbD::DispCompIecJecIe::DispCompIecJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis) { } + +void MbD::DispCompIecJecIe::calc_value() +{ + aAjOIe = frmI->aAjOe(axis); + rIeJeO = frmJ->rOeO->minusFullColumn(frmI->rOeO); + riIeJeIe = aAjOIe->dot(rIeJeO); +} + +void MbD::DispCompIecJecIe::calcPostDynCorrectorIteration() +{ + calc_value(); +} + +double MbD::DispCompIecJecIe::value() +{ + return riIeJeIe; +} diff --git a/MbDCode/DispCompIecJecIe.h b/MbDCode/DispCompIecJecIe.h index 62f4da9..0e5e801 100644 --- a/MbDCode/DispCompIecJecIe.h +++ b/MbDCode/DispCompIecJecIe.h @@ -10,6 +10,10 @@ namespace MbD { DispCompIecJecIe(); DispCompIecJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + void calc_value() override; + void calcPostDynCorrectorIteration() override; + double value() override; + int axis; double riIeJeIe; FColDsptr aAjOIe, rIeJeO; diff --git a/MbDCode/DispCompIeqcJecIe.cpp b/MbDCode/DispCompIeqcJecIe.cpp index 8fc75c0..6d65120 100644 --- a/MbDCode/DispCompIeqcJecIe.cpp +++ b/MbDCode/DispCompIeqcJecIe.cpp @@ -1,4 +1,7 @@ #include "DispCompIeqcJecIe.h" +#include "EndFrameqc.h" + +using namespace MbD; MbD::DispCompIeqcJecIe::DispCompIeqcJecIe() { @@ -7,3 +10,101 @@ MbD::DispCompIeqcJecIe::DispCompIeqcJecIe() MbD::DispCompIeqcJecIe::DispCompIeqcJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecIe(frmi, frmj, axis) { } + +void MbD::DispCompIeqcJecIe::calc_ppvaluepEIpEI() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + auto mprIeJeOpEIT = frmIeqc->prOeOpE->transpose(); + auto mpprIeJeOpEIpEI = frmIeqc->pprOeOpEpE; + for (int i = 0; i < 4; i++) + { + auto ppAjOIepEIipEI = ppAjOIepEIpEI->at(i); + auto mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i); + auto ppriIeJeIepEIipEI = ppriIeJeIepEIpEI->at(i); + for (int j = i; j < 4; j++) + { + auto term1 = ppAjOIepEIipEI->at(j)->dot(rIeJeO); + auto mterm2 = pAjOIepEIT->at(i)->dot(mprIeJeOpEIT->at(j)); + auto mterm3 = (i == j) ? mterm2 : pAjOIepEIT->at(j)->dot(mprIeJeOpEIT->at(i)); + auto mterm4 = aAjOIe->dot(mpprIeJeOpEIipEI->at(j)); + ppriIeJeIepEIipEI->atiput(j, term1 - mterm2 - mterm3 - mterm4); + } + } + ppriIeJeIepEIpEI->symLowerWithUpper(); +} + +void MbD::DispCompIeqcJecIe::calc_ppvaluepXIpEI() +{ + for (int i = 0; i < 3; i++) + { + auto ppriIeJeIepXIipEI = ppriIeJeIepXIpEI->at(i); + for (int j = 0; j < 4; j++) + { + ppriIeJeIepXIipEI->atiput(j, -pAjOIepEIT->at(j)->at(i)); + } + } +} + +void MbD::DispCompIeqcJecIe::calc_pvaluepEI() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + pAjOIepEIT = frmIeqc->pAjOepET(axis); + auto mprIeJeOpEIT = frmIeqc->prOeOpE->transpose(); + for (int i = 0; i < 4; i++) + { + priIeJeIepEI->atiput(i, pAjOIepEIT->at(i)->dot(rIeJeO) - aAjOIe->dot(mprIeJeOpEIT->at(i))); + } +} + +void MbD::DispCompIeqcJecIe::calc_pvaluepXI() +{ + for (int i = 0; i < 3; i++) + { + priIeJeIepXI->atiput(i, -aAjOIe->at(i)); + } +} + +void MbD::DispCompIeqcJecIe::calcPostDynCorrectorIteration() +{ + //Must maintain order of calc_xxx. + DispCompIecJecIe::calcPostDynCorrectorIteration(); + calc_pvaluepXI(); + calc_pvaluepEI(); + calc_ppvaluepXIpEI(); + calc_ppvaluepEIpEI(); +} + +void MbD::DispCompIeqcJecIe::initialize() +{ + DispCompIecJecIe::initialize(); + priIeJeIepXI = std::make_shared>(3); + priIeJeIepEI = std::make_shared>(4); + ppriIeJeIepXIpEI = std::make_shared>(3, 4); + ppriIeJeIepEIpEI = std::make_shared>(4, 4); +} + +void MbD::DispCompIeqcJecIe::initializeGlobally() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + ppAjOIepEIpEI = frmIeqc->ppAjOepEpE(axis); +} + +FMatDsptr MbD::DispCompIeqcJecIe::ppvaluepEIpEI() +{ + return ppriIeJeIepEIpEI; +} + +FMatDsptr MbD::DispCompIeqcJecIe::ppvaluepXIpEI() +{ + return ppriIeJeIepXIpEI; +} + +FRowDsptr MbD::DispCompIeqcJecIe::pvaluepEI() +{ + return priIeJeIepEI; +} + +FRowDsptr MbD::DispCompIeqcJecIe::pvaluepXI() +{ + return priIeJeIepXI; +} diff --git a/MbDCode/DispCompIeqcJecIe.h b/MbDCode/DispCompIeqcJecIe.h index 6c52bf6..62638a6 100644 --- a/MbDCode/DispCompIeqcJecIe.h +++ b/MbDCode/DispCompIeqcJecIe.h @@ -10,6 +10,18 @@ namespace MbD { DispCompIeqcJecIe(); DispCompIeqcJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + void calc_ppvaluepEIpEI() override; + void calc_ppvaluepXIpEI() override; + void calc_pvaluepEI() override; + void calc_pvaluepXI() override; + void calcPostDynCorrectorIteration() override; + void initialize() override; + void initializeGlobally() override; + FMatDsptr ppvaluepEIpEI() override; + FMatDsptr ppvaluepXIpEI() override; + FRowDsptr pvaluepEI() override; + FRowDsptr pvaluepXI() override; + FRowDsptr priIeJeIepXI, priIeJeIepEI; FMatDsptr ppriIeJeIepXIpEI, ppriIeJeIepEIpEI, pAjOIepEIT; FMatFColDsptr ppAjOIepEIpEI; diff --git a/MbDCode/DispCompIeqcJeqcIe.cpp b/MbDCode/DispCompIeqcJeqcIe.cpp index 85701dc..54e82e7 100644 --- a/MbDCode/DispCompIeqcJeqcIe.cpp +++ b/MbDCode/DispCompIeqcJeqcIe.cpp @@ -1,4 +1,7 @@ #include "DispCompIeqcJeqcIe.h" +#include "EndFrameqc.h" + +using namespace MbD; MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe() { @@ -7,3 +10,97 @@ MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe() MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecIe(frmi, frmj, axis) { } + +void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpEJ() +{ + auto frmJeqc = std::static_pointer_cast(frmJ); + auto& prIeJeOpEJ = frmJeqc->prOeOpE; + ppriIeJeIepEIpEJ = pAjOIepEIT->timesFullMatrix(prIeJeOpEJ); +} + +void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpXJ() +{ + ppriIeJeIepEIpXJ = pAjOIepEIT; +} + +void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEJpEJ() +{ + auto frmJeqc = std::static_pointer_cast(frmJ); + auto pprIeJeOpEJpEJ = frmJeqc->pprOeOpEpE; + for (int i = 0; i < 4; i++) + { + auto pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i); + auto ppriIeJeIepEJipEJ = ppriIeJeIepEJpEJ->at(i); + for (int j = i; j < 4; j++) + { + auto term1 = aAjOIe->dot(pprIeJeOpEJipEJ->at(j)); + ppriIeJeIepEJipEJ->atiput(j, term1); + } + } + ppriIeJeIepEJpEJ->symLowerWithUpper(); +} + +void MbD::DispCompIeqcJeqcIe::calc_pvaluepEJ() +{ + auto frmJeqc = std::static_pointer_cast(frmJ); + auto prIeJeOpEJT = frmJeqc->prOeOpE->transpose(); + for (int i = 0; i < 4; i++) + { + priIeJeIepEJ->atiput(i, aAjOIe->dot(prIeJeOpEJT->at(i))); + } +} + +void MbD::DispCompIeqcJeqcIe::calc_pvaluepXJ() +{ + for (int i = 0; i < 3; i++) + { + priIeJeIepXJ->atiput(i, aAjOIe->at(i)); + } +} + +void MbD::DispCompIeqcJeqcIe::calcPostDynCorrectorIteration() +{ + //Must maintain order of calc_xxx. + DispCompIeqcJecIe::calcPostDynCorrectorIteration(); + calc_pvaluepXJ(); + calc_pvaluepEJ(); + calc_ppvaluepEIpXJ(); + calc_ppvaluepEIpEJ(); + calc_ppvaluepEJpEJ(); + +} + +void MbD::DispCompIeqcJeqcIe::initialize() +{ + DispCompIeqcJecIe::initialize(); + priIeJeIepXJ = std::make_shared>(3); + priIeJeIepEJ = std::make_shared>(4); + ppriIeJeIepEIpXJ = std::make_shared>(4, 3); + ppriIeJeIepEIpEJ = std::make_shared>(4, 4); + ppriIeJeIepEJpEJ = std::make_shared>(4, 4); +} + +FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpEJ() +{ + return ppriIeJeIepEIpEJ; +} + +FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpXJ() +{ + return ppriIeJeIepEIpXJ; +} + +FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEJpEJ() +{ + return ppriIeJeIepEJpEJ; +} + +FRowDsptr MbD::DispCompIeqcJeqcIe::pvaluepEJ() +{ + return priIeJeIepEJ; +} + +FRowDsptr MbD::DispCompIeqcJeqcIe::pvaluepXJ() +{ + return priIeJeIepXJ; +} diff --git a/MbDCode/DispCompIeqcJeqcIe.h b/MbDCode/DispCompIeqcJeqcIe.h index aafbcf0..9d2dfb6 100644 --- a/MbDCode/DispCompIeqcJeqcIe.h +++ b/MbDCode/DispCompIeqcJeqcIe.h @@ -10,6 +10,19 @@ namespace MbD { DispCompIeqcJeqcIe(); DispCompIeqcJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + void calc_ppvaluepEIpEJ() override; + void calc_ppvaluepEIpXJ() override; + void calc_ppvaluepEJpEJ() override; + void calc_pvaluepEJ() override; + void calc_pvaluepXJ() override; + void calcPostDynCorrectorIteration() override; + void initialize() override; + FMatDsptr ppvaluepEIpEJ() override; + FMatDsptr ppvaluepEIpXJ() override; + FMatDsptr ppvaluepEJpEJ() override; + FRowDsptr pvaluepEJ() override; + FRowDsptr pvaluepXJ() override; + FRowDsptr priIeJeIepXJ, priIeJeIepEJ; FMatDsptr ppriIeJeIepEIpXJ, ppriIeJeIepEIpEJ, ppriIeJeIepEJpEJ; }; diff --git a/MbDCode/DispCompIeqctJeqcIe.cpp b/MbDCode/DispCompIeqctJeqcIe.cpp index d6f6e8c..fc3460c 100644 --- a/MbDCode/DispCompIeqctJeqcIe.cpp +++ b/MbDCode/DispCompIeqctJeqcIe.cpp @@ -1,4 +1,7 @@ #include "DispCompIeqctJeqcIe.h" +#include "EndFrameqct.h" + +using namespace MbD; MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe() { @@ -7,3 +10,140 @@ MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe() MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcIe(frmi, frmj, axis) { } + +void MbD::DispCompIeqctJeqcIe::calc_ppvaluepEIpt() +{ + auto frmIeqct = std::static_pointer_cast(frmI); + auto pAjOIept = frmIeqct->pAjOept(axis); + auto ppAjOIepEITpt = frmIeqct->ppAjOepETpt(axis); + auto mprIeJeOpEIT = frmIeqct->prOeOpE->transpose(); + auto mpprIeJeOpEITpt = frmIeqct->pprOeOpEpt->transpose(); + auto mprIeJeOpt = frmIeqct->prOeOpt; + for (int i = 0; i < 4; i++) + { + auto ppAjOIepEITpti = ppAjOIepEITpt->at(i); + auto pAjOIepEITi = pAjOIepEIT->at(i); + auto mprIeJeOpEITi = mprIeJeOpEIT->at(i); + auto mpprIeJeOpEITpti = mpprIeJeOpEITpt->at(i); + auto ppriIeJeIepEIpti = ppAjOIepEITpti->dot(rIeJeO) - pAjOIepEITi->dot(mprIeJeOpt) - pAjOIept->dot(mprIeJeOpEITi) - aAjOIe->dot(mpprIeJeOpEITpti); + ppriIeJeIepEIpt->atiput(i, ppriIeJeIepEIpti); + } +} + +void MbD::DispCompIeqctJeqcIe::calc_ppvaluepEJpt() +{ + auto frmIeqct = std::static_pointer_cast(frmI); + auto frmJeqct = std::static_pointer_cast(frmJ); + auto pAjOIept = frmIeqct->pAjOept(axis); + auto prIeJeOpEJT = frmJeqct->prOeOpE->transpose(); + for (int i = 0; i < 4; i++) + { + ppriIeJeIepEJpt->atiput(i, pAjOIept->dot(prIeJeOpEJT->at(i))); + } +} + +void MbD::DispCompIeqctJeqcIe::calc_ppvalueptpt() +{ + auto frmIeqct = std::static_pointer_cast(frmI); + auto pAjOIept = frmIeqct->pAjOept(axis); + auto ppAjOIeptpt = frmIeqct->ppAjOeptpt(axis); + auto mprIeJeOpt = frmIeqct->prOeOpt; + auto mpprIeJeOptpt = frmIeqct->pprOeOptpt; + ppriIeJeIeptpt = ppAjOIeptpt->dot(rIeJeO) - pAjOIept->dot(mprIeJeOpt) - pAjOIept->dot(mprIeJeOpt) - aAjOIe->dot(mpprIeJeOptpt); +} + +void MbD::DispCompIeqctJeqcIe::calc_ppvaluepXIpt() +{ + auto frmIeqct = std::static_pointer_cast(frmI); + auto pAjOIept = frmIeqct->pAjOept(axis); + for (int i = 0; i < 3; i++) + { + ppriIeJeIepXIpt->atiput(i, -pAjOIept->at(i)); + } +} + +void MbD::DispCompIeqctJeqcIe::calc_ppvaluepXJpt() +{ + auto frmIeqct = std::static_pointer_cast(frmI); + auto pAjOIept = frmIeqct->pAjOept(axis); + for (int i = 0; i < 3; i++) + { + ppriIeJeIepXJpt->atiput(i, pAjOIept->at(i)); + } +} + +void MbD::DispCompIeqctJeqcIe::calc_pvaluept() +{ + auto frmIeqct = std::static_pointer_cast(frmI); + auto pAjOIept = frmIeqct->pAjOept(axis); + auto mprIeJeOpt = frmIeqct->prOeOpt; + priIeJeIept = pAjOIept->dot(rIeJeO) - aAjOIe->dot(mprIeJeOpt); +} + +void MbD::DispCompIeqctJeqcIe::calcPostDynCorrectorIteration() +{ + //"ppAjOIepEIpEI is not longer constant and must be set before any calculation." + auto frmIeqct = std::static_pointer_cast(frmI); + ppAjOIepEIpEI = frmIeqct->ppAjOepEpE(axis); + DispCompIeqcJeqcIe::calcPostDynCorrectorIteration(); +} + +void MbD::DispCompIeqctJeqcIe::initialize() +{ + DispCompIeqcJeqcIe::initialize(); + ppriIeJeIepXIpt = std::make_shared>(3); + ppriIeJeIepEIpt = std::make_shared>(4); + ppriIeJeIepXJpt = std::make_shared>(3); + ppriIeJeIepEJpt = std::make_shared>(4); +} + +void MbD::DispCompIeqctJeqcIe::initializeGlobally() +{ + //"Do nothing." +} + +void MbD::DispCompIeqctJeqcIe::preAccIC() +{ + DispCompIeqcJeqcIe::preAccIC(); + calc_ppvaluepXIpt(); + calc_ppvaluepEIpt(); + calc_ppvaluepXJpt(); + calc_ppvaluepEJpt(); + calc_ppvalueptpt(); +} + +void MbD::DispCompIeqctJeqcIe::preVelIC() +{ + DispCompIeqcJeqcIe::preVelIC(); + calc_pvaluept(); +} + +FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepEIpt() +{ + return ppriIeJeIepEIpt; +} + +FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepEJpt() +{ + return ppriIeJeIepEJpt; +} + +double MbD::DispCompIeqctJeqcIe::ppvalueptpt() +{ + return ppriIeJeIeptpt; +} + +FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepXIpt() +{ + return ppriIeJeIepXIpt; +} + +FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepXJpt() +{ + return ppriIeJeIepXJpt; +} + +double MbD::DispCompIeqctJeqcIe::pvaluept() +{ + return priIeJeIept; +} diff --git a/MbDCode/DispCompIeqctJeqcIe.h b/MbDCode/DispCompIeqctJeqcIe.h index 9bd8384..21d7013 100644 --- a/MbDCode/DispCompIeqctJeqcIe.h +++ b/MbDCode/DispCompIeqctJeqcIe.h @@ -10,6 +10,24 @@ namespace MbD { DispCompIeqctJeqcIe(); DispCompIeqctJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + void calc_ppvaluepEIpt() override; + void calc_ppvaluepEJpt() override; + void calc_ppvalueptpt() override; + void calc_ppvaluepXIpt() override; + void calc_ppvaluepXJpt() override; + void calc_pvaluept() override; + void calcPostDynCorrectorIteration() override; + void initialize() override; + void initializeGlobally() override; + void preAccIC() override; + void preVelIC() override; + FRowDsptr ppvaluepEIpt() override; + FRowDsptr ppvaluepEJpt() override; + double ppvalueptpt() override; + FRowDsptr ppvaluepXIpt() override; + FRowDsptr ppvaluepXJpt() override; + double pvaluept() override; + double priIeJeIept, ppriIeJeIeptpt; FRowDsptr ppriIeJeIepXIpt, ppriIeJeIepEIpt, ppriIeJeIepXJpt, ppriIeJeIepEJpt; }; diff --git a/MbDCode/DistIecJec.cpp b/MbDCode/DistIecJec.cpp index 3e27b16..fc40756 100644 --- a/MbDCode/DistIecJec.cpp +++ b/MbDCode/DistIecJec.cpp @@ -1,5 +1,7 @@ #include "DistIecJec.h" +using namespace MbD; + MbD::DistIecJec::DistIecJec() { } diff --git a/MbDCode/DistanceConstraintIJ.cpp b/MbDCode/DistanceConstraintIJ.cpp index 4bd0467..a52013e 100644 --- a/MbDCode/DistanceConstraintIJ.cpp +++ b/MbDCode/DistanceConstraintIJ.cpp @@ -11,6 +11,11 @@ void MbD::DistanceConstraintIJ::calcPostDynCorrectorIteration() aG = distIeJe->value() - aConstant; } +void MbD::DistanceConstraintIJ::init_distIeJe() +{ + assert(false); +} + void MbD::DistanceConstraintIJ::initialize() { ConstraintIJ::initialize(); diff --git a/MbDCode/DistanceConstraintIJ.h b/MbDCode/DistanceConstraintIJ.h index 2705d23..3836ffc 100644 --- a/MbDCode/DistanceConstraintIJ.h +++ b/MbDCode/DistanceConstraintIJ.h @@ -11,7 +11,7 @@ namespace MbD { DistanceConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); void calcPostDynCorrectorIteration() override; - virtual void init_distIeJe() = 0; + virtual void init_distIeJe(); void initialize() override; void initializeGlobally() override; void initializeLocally() override; diff --git a/MbDCode/DistanceConstraintIqcJc.cpp b/MbDCode/DistanceConstraintIqcJc.cpp index 582caf5..10115f6 100644 --- a/MbDCode/DistanceConstraintIqcJc.cpp +++ b/MbDCode/DistanceConstraintIqcJc.cpp @@ -3,6 +3,8 @@ #include "CREATE.h" #include "DistIeqcJec.h" +using namespace MbD; + MbD::DistanceConstraintIqcJc::DistanceConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIJ(frmi, frmj) { } diff --git a/MbDCode/DistanceConstraintIqcJqc.cpp b/MbDCode/DistanceConstraintIqcJqc.cpp index 4a1f5b6..f5a9fe9 100644 --- a/MbDCode/DistanceConstraintIqcJqc.cpp +++ b/MbDCode/DistanceConstraintIqcJqc.cpp @@ -3,6 +3,8 @@ #include "CREATE.h" #include "DistIeqcJeqc.h" +using namespace MbD; + MbD::DistanceConstraintIqcJqc::DistanceConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIqcJc(frmi, frmj) { } diff --git a/MbDCode/DistanceConstraintIqctJqc.cpp b/MbDCode/DistanceConstraintIqctJqc.cpp index 84941b9..7871b2b 100644 --- a/MbDCode/DistanceConstraintIqctJqc.cpp +++ b/MbDCode/DistanceConstraintIqctJqc.cpp @@ -1,6 +1,13 @@ #include "DistanceConstraintIqctJqc.h" +using namespace MbD; + MbD::DistanceConstraintIqctJqc::DistanceConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIqcJqc(frmi, frmj) { assert(false); } + +ConstraintType MbD::DistanceConstraintIqctJqc::type() +{ + return essential; +} diff --git a/MbDCode/DistanceConstraintIqctJqc.h b/MbDCode/DistanceConstraintIqctJqc.h index a747d04..1bf5657 100644 --- a/MbDCode/DistanceConstraintIqctJqc.h +++ b/MbDCode/DistanceConstraintIqctJqc.h @@ -8,6 +8,7 @@ namespace MbD { //pGpt ppGpXIpt ppGpEIpt ppGpXJpt ppGpEJpt ppGptpt public: DistanceConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj); + ConstraintType type() override; double pGpt, ppGptpt; FRowDsptr ppGpXIpt, ppGpEIpt, ppGpXJpt, ppGpEJpt; diff --git a/MbDCode/DistancexyConstraintIJ.cpp b/MbDCode/DistancexyConstraintIJ.cpp index 0a45f2a..bc7cf42 100644 --- a/MbDCode/DistancexyConstraintIJ.cpp +++ b/MbDCode/DistancexyConstraintIJ.cpp @@ -13,6 +13,11 @@ void MbD::DistancexyConstraintIJ::calcPostDynCorrectorIteration() aG = x * x + (y * y) - (aConstant * aConstant); } +void MbD::DistancexyConstraintIJ::init_xyIeJeIe() +{ + assert(false); +} + void MbD::DistancexyConstraintIJ::initialize() { ConstraintIJ::initialize(); @@ -75,5 +80,5 @@ void MbD::DistancexyConstraintIJ::simUpdateAll() ConstraintType MbD::DistancexyConstraintIJ::type() { - return ConstraintType::displacement; + return displacement; } diff --git a/MbDCode/DistancexyConstraintIJ.h b/MbDCode/DistancexyConstraintIJ.h index 73f6f79..a4d3500 100644 --- a/MbDCode/DistancexyConstraintIJ.h +++ b/MbDCode/DistancexyConstraintIJ.h @@ -11,7 +11,7 @@ namespace MbD { DistancexyConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); void calcPostDynCorrectorIteration() override; - virtual void init_xyIeJeIe() = 0; + virtual void init_xyIeJeIe(); void initialize() override; void initializeGlobally() override; void initializeLocally() override; @@ -26,6 +26,7 @@ namespace MbD { std::shared_ptr xIeJeIe, yIeJeIe; + //ToDo: Use DistxyIecJec instead of xIeJeIe, yIeJeIe }; } diff --git a/MbDCode/DistancexyConstraintIqcJc.h b/MbDCode/DistancexyConstraintIqcJc.h index 9f09342..3225c77 100644 --- a/MbDCode/DistancexyConstraintIqcJc.h +++ b/MbDCode/DistancexyConstraintIqcJc.h @@ -17,12 +17,12 @@ namespace MbD { void calc_ppGpXIpEI(); void calc_ppGpXIpXI(); void calcPostDynCorrectorIteration() override; - void init_xyIeJeIe() override; void fillAccICIterError(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; + void init_xyIeJeIe() override; void useEquationNumbers() override; diff --git a/MbDCode/DistxyIecJec.cpp b/MbDCode/DistxyIecJec.cpp new file mode 100644 index 0000000..b9be770 --- /dev/null +++ b/MbDCode/DistxyIecJec.cpp @@ -0,0 +1,88 @@ +#include "DistxyIecJec.h" + +using namespace MbD; + +MbD::DistxyIecJec::DistxyIecJec() +{ +} + +MbD::DistxyIecJec::DistxyIecJec(EndFrmcptr frmi, EndFrmcptr frmj) : KinematicIeJe(frmi, frmj) +{ +} + +void MbD::DistxyIecJec::calcPostDynCorrectorIteration() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + distxy = std::sqrt(x * x + (y * y)); +} + +void MbD::DistxyIecJec::initialize() +{ + KinematicIeJe::initialize(); + this->init_xyIeJeIe(); +} + +void MbD::DistxyIecJec::initializeGlobally() +{ + xIeJeIe->initializeGlobally(); + yIeJeIe->initializeGlobally(); +} + +void MbD::DistxyIecJec::initializeLocally() +{ + xIeJeIe->initializeLocally(); + yIeJeIe->initializeLocally(); +} + +void MbD::DistxyIecJec::init_xyIeJeIe() +{ + assert(false); +} + +void MbD::DistxyIecJec::postInput() +{ + xIeJeIe->postInput(); + yIeJeIe->postInput(); + KinematicIeJe::postInput(); +} + +void MbD::DistxyIecJec::postPosICIteration() +{ + xIeJeIe->postPosICIteration(); + yIeJeIe->postPosICIteration(); + KinematicIeJe::postPosICIteration(); +} + +void MbD::DistxyIecJec::preAccIC() +{ + xIeJeIe->preAccIC(); + yIeJeIe->preAccIC(); + KinematicIeJe::preAccIC(); +} + +void MbD::DistxyIecJec::prePosIC() +{ + xIeJeIe->prePosIC(); + yIeJeIe->prePosIC(); + KinematicIeJe::prePosIC(); +} + +void MbD::DistxyIecJec::preVelIC() +{ + xIeJeIe->preVelIC(); + yIeJeIe->preVelIC(); + KinematicIeJe::preVelIC(); +} + +void MbD::DistxyIecJec::simUpdateAll() +{ + xIeJeIe->simUpdateAll(); + yIeJeIe->simUpdateAll(); + KinematicIeJe::simUpdateAll(); +} + +double MbD::DistxyIecJec::value() +{ + return distxy; +} diff --git a/MbDCode/DistxyIecJec.h b/MbDCode/DistxyIecJec.h new file mode 100644 index 0000000..04a6775 --- /dev/null +++ b/MbDCode/DistxyIecJec.h @@ -0,0 +1,32 @@ +#pragma once + +#include "KinematicIeJe.h" +#include "DispCompIecJecIe.h" + +namespace MbD { + class DistxyIecJec : public KinematicIeJe + { + //distxy xIeJeIe yIeJeIe + public: + DistxyIecJec(); + DistxyIecJec(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + virtual void init_xyIeJeIe(); + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + double value() override; + + double distxy; + std::shared_ptr xIeJeIe, yIeJeIe; + + }; +} + diff --git a/MbDCode/DistxyIeqcJec.cpp b/MbDCode/DistxyIeqcJec.cpp new file mode 100644 index 0000000..f680b33 --- /dev/null +++ b/MbDCode/DistxyIeqcJec.cpp @@ -0,0 +1,184 @@ +#include "DistxyIeqcJec.h" +#include "CREATE.h" +#include "DispCompIeqcJecIe.h" + +using namespace MbD; + +MbD::DistxyIeqcJec::DistxyIeqcJec() +{ +} + +MbD::DistxyIeqcJec::DistxyIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : DistxyIecJec(frmi, frmj) +{ +} + +void MbD::DistxyIeqcJec::calc_ppdistxypEIpEI() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto ppxpEIpEI = xIeJeIe->ppvaluepEIpEI(); + auto ppypEIpEI = yIeJeIe->ppvaluepEIpEI(); + for (int i = 0; i < 4; i++) + { + auto ppdistxypEIpEIi = ppdistxypEIpEI->at(i); + auto pdistxypEIi = pdistxypEI->at(i); + auto ppxpEIpEIi = ppxpEIpEI->at(i); + auto ppypEIpEIi = ppypEIpEI->at(i); + auto pxpEIi = pxpEI->at(i); + auto pypEIi = pypEI->at(i); + for (int j = i; j < 4; j++) + { + auto pdistxypEIj = pdistxypEI->at(j); + auto pxpEIj = pxpEI->at(j); + auto pypEIj = pypEI->at(j); + auto term1 = -pdistxypEIi * pdistxypEIj; + auto term2 = ppxpEIpEIi->at(j) * x + ppypEIpEIi->at(j) * y; + auto term3 = pxpEIi * pxpEIj + pypEIi * pypEIj; + auto ppdistxypEIpEIij = (term1 + term2 + term3) / distxy; + ppdistxypEIpEIi->atiput(j, ppdistxypEIpEIij); + if (i < j) ppdistxypEIpEI->atijput(j, i, ppdistxypEIpEIij); + } + } +} + +void MbD::DistxyIeqcJec::calc_ppdistxypXIpEI() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto ppxpXIpEI = xIeJeIe->ppvaluepXIpEI(); + auto ppypXIpEI = yIeJeIe->ppvaluepXIpEI(); + for (int i = 0; i < 3; i++) + { + auto ppdistxypXIpEIi = ppdistxypXIpEI->at(i); + auto pdistxypXIi = pdistxypXI->at(i); + auto ppxpXIpEIi = ppxpXIpEI->at(i); + auto ppypXIpEIi = ppypXIpEI->at(i); + auto pxpXIi = pxpXI->at(i); + auto pypXIi = pypXI->at(i); + for (int j = 0; j < 4; j++) + { + auto pdistxypEIj = pdistxypEI->at(j); + auto pxpEIj = pxpEI->at(j); + auto pypEIj = pypEI->at(j); + auto term1 = -pdistxypXIi * pdistxypEIj; + auto term2 = ppxpXIpEIi->at(j) * x + ppypXIpEIi->at(j) * y; + auto term3 = pxpXIi * pxpEIj + pypXIi * pypEIj; + auto ppdistxypXIpEIij = (term1 + term2 + term3) / distxy; + ppdistxypXIpEIi->atiput(j, ppdistxypXIpEIij); + } + } +} + +void MbD::DistxyIeqcJec::calc_ppdistxypXIpXI() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + auto ppxpXIpXI = xIeJeIe->ppvaluepXIpXI(); + auto ppypXIpXI = yIeJeIe->ppvaluepXIpXI(); + for (int i = 0; i < 3; i++) + { + auto ppdistxypXIpXIi = ppdistxypXIpXI->at(i); + auto pdistxypXIi = pdistxypXI->at(i); + auto ppxpXIpXIi = ppxpXIpXI->at(i); + auto ppypXIpXIi = ppypXIpXI->at(i); + auto pxpXIi = pxpXI->at(i); + auto pypXIi = pypXI->at(i); + for (int j = i; j < 3; j++) + { + auto pdistxypXIj = pdistxypXI->at(j); + auto pxpXIj = pxpXI->at(j); + auto pypXIj = pypXI->at(j); + auto term1 = -pdistxypXIi * pdistxypXIj; + auto term2 = ppxpXIpXIi->at(j) * x + ppypXIpXIi->at(j) * y; + auto term3 = pxpXIi * pxpXIj + pypXIi * pypXIj; + auto ppdistxypXIpXIij = (term1 + term2 + term3) / distxy; + ppdistxypXIpXIi->atiput(j, ppdistxypXIpXIij); + if (i < j) ppdistxypXIpXI->atijput(j, i, ppdistxypXIpXIij); + } + } +} + +void MbD::DistxyIeqcJec::calc_pdistxypEI() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + for (int i = 0; i < 4; i++) + { + auto term = pxpEI->at(i) * x + pypEI->at(i) * y; + pdistxypEI->atiput(i, term / distxy); + } +} + +void MbD::DistxyIeqcJec::calc_pdistxypXI() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + for (int i = 0; i < 3; i++) + { + auto term = pxpXI->at(i) * x + pypXI->at(i) * y; + pdistxypXI->atiput(i, term / distxy); + } +} + +void MbD::DistxyIeqcJec::calcPostDynCorrectorIteration() +{ + DistxyIeqcJec::calcPostDynCorrectorIteration(); + this->calc_pdistxypXI(); + this->calc_pdistxypEI(); + this->calc_ppdistxypXIpXI(); + this->calc_ppdistxypXIpEI(); + this->calc_ppdistxypEIpEI(); +} + +void MbD::DistxyIeqcJec::init_xyIeJeIe() +{ + xIeJeIe = CREATE::With(frmI, frmJ, 0); + yIeJeIe = CREATE::With(frmI, frmJ, 1); +} + +void MbD::DistxyIeqcJec::initialize() +{ + DistxyIecJec::initialize(); + pdistxypXI = std::make_shared>(3); + pdistxypEI = std::make_shared>(4); + ppdistxypXIpXI = std::make_shared>(3, 3); + ppdistxypXIpEI = std::make_shared>(3, 4); + ppdistxypEIpEI = std::make_shared>(4, 4); +} + +FMatDsptr MbD::DistxyIeqcJec::ppvaluepEIpEI() +{ + return ppdistxypEIpEI; +} + +FMatDsptr MbD::DistxyIeqcJec::ppvaluepXIpEI() +{ + return ppdistxypXIpEI; +} + +FMatDsptr MbD::DistxyIeqcJec::ppvaluepXIpXI() +{ + return ppdistxypXIpXI; +} + +FRowDsptr MbD::DistxyIeqcJec::pvaluepEI() +{ + return pdistxypEI; +} + +FRowDsptr MbD::DistxyIeqcJec::pvaluepXI() +{ + return pdistxypXI; +} diff --git a/MbDCode/DistxyIeqcJec.h b/MbDCode/DistxyIeqcJec.h new file mode 100644 index 0000000..3ed23a4 --- /dev/null +++ b/MbDCode/DistxyIeqcJec.h @@ -0,0 +1,32 @@ +#pragma once + +#include "DistxyIecJec.h" + +namespace MbD { + class DistxyIeqcJec : public DistxyIecJec + { + //pdistxypXI pdistxypEI ppdistxypXIpXI ppdistxypXIpEI ppdistxypEIpEI + + public: + DistxyIeqcJec(); + DistxyIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj); + + void calc_ppdistxypEIpEI(); + void calc_ppdistxypXIpEI(); + void calc_ppdistxypXIpXI(); + void calc_pdistxypEI(); + void calc_pdistxypXI(); + void calcPostDynCorrectorIteration() override; + void init_xyIeJeIe() override; + void initialize() override; + FMatDsptr ppvaluepEIpEI() override; + FMatDsptr ppvaluepXIpEI() override; + FMatDsptr ppvaluepXIpXI() override; + FRowDsptr pvaluepEI() override; + FRowDsptr pvaluepXI() override; + + FRowDsptr pdistxypXI, pdistxypEI; + FMatDsptr ppdistxypXIpXI, ppdistxypXIpEI, ppdistxypEIpEI; + }; +} + diff --git a/MbDCode/DistxyIeqcJeqc.cpp b/MbDCode/DistxyIeqcJeqc.cpp new file mode 100644 index 0000000..f2ab3e9 --- /dev/null +++ b/MbDCode/DistxyIeqcJeqc.cpp @@ -0,0 +1,340 @@ +#include "DistxyIeqcJeqc.h" +#include "CREATE.h" +#include "DispCompIeqcJeqcIe.h" + +using namespace MbD; + +MbD::DistxyIeqcJeqc::DistxyIeqcJeqc() +{ +} + +MbD::DistxyIeqcJeqc::DistxyIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistxyIeqcJec(frmi, frmj) +{ +} + +void MbD::DistxyIeqcJeqc::calc_ppdistxypEIpEJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + auto ppxpEIpEJ = xIeJeIe->ppvaluepEIpEJ(); + auto ppypEIpEJ = yIeJeIe->ppvaluepEIpEJ(); + for (int i = 0; i < 4; i++) + { + auto& ppdistxypEIpEJi = ppdistxypEIpEJ->at(i); + auto& pdistxypEIi = pdistxypEI->at(i); + auto& ppxpEIpEJi = ppxpEIpEJ->at(i); + auto& ppypEIpEJi = ppypEIpEJ->at(i); + auto& pxpEIi = pxpEI->at(i); + auto& pypEIi = pypEI->at(i); + for (int j = 0; j < 4; j++) + { + auto& pdistxypEJj = pdistxypEJ->at(j); + auto& pxpEJj = pxpEJ->at(j); + auto& pypEJj = pypEJ->at(j); + auto term1 = -pdistxypEIi * pdistxypEJj; + auto term2 = ppxpEIpEJi->at(j) * x + ppypEIpEJi->at(j) * y; + auto term3 = pxpEIi * pxpEJj + pypEIi * pypEJj; + auto ppdistxypEIpEJij = (term1 + term2 + term3) / distxy; + ppdistxypEIpEJi->atiput(j, ppdistxypEIpEJij); + } + } +} + +void MbD::DistxyIeqcJeqc::calc_ppdistxypEIpXJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + auto ppxpEIpXJ = xIeJeIe->ppvaluepEIpXJ(); + auto ppypEIpXJ = yIeJeIe->ppvaluepEIpXJ(); + for (int i = 0; i < 4; i++) + { + auto& ppdistxypEIpXJi = ppdistxypEIpXJ->at(i); + auto& pdistxypEIi = pdistxypEI->at(i); + auto& ppxpEIpXJi = ppxpEIpXJ->at(i); + auto& ppypEIpXJi = ppypEIpXJ->at(i); + auto& pxpEIi = pxpEI->at(i); + auto& pypEIi = pypEI->at(i); + for (int j = 0; j < 3; j++) + { + auto& pdistxypXJj = pdistxypXJ->at(j); + auto& pxpXJj = pxpXJ->at(j); + auto& pypXJj = pypXJ->at(j); + auto term1 = -pdistxypEIi * pdistxypXJj; + auto term2 = ppxpEIpXJi->at(j) * x + ppypEIpXJi->at(j) * y; + auto term3 = pxpEIi * pxpXJj + pypEIi * pypXJj; + auto ppdistxypEIpXJij = (term1 + term2 + term3) / distxy; + ppdistxypEIpXJi->atiput(j, ppdistxypEIpXJij); + } + } +} + +void MbD::DistxyIeqcJeqc::calc_ppdistxypEJpEJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + auto ppxpEJpEJ = xIeJeIe->ppvaluepEJpEJ(); + auto ppypEJpEJ = yIeJeIe->ppvaluepEJpEJ(); + for (int i = 0; i < 4; i++) + { + auto& ppdistxypEJpEJi = ppdistxypEJpEJ->at(i); + auto& pdistxypEJi = pdistxypEJ->at(i); + auto& ppxpEJpEJi = ppxpEJpEJ->at(i); + auto& ppypEJpEJi = ppypEJpEJ->at(i); + auto& pxpEJi = pxpEJ->at(i); + auto& pypEJi = pypEJ->at(i); + for (int j = i; j < 4; j++) + { + auto& pdistxypEJj = pdistxypEJ->at(j); + auto& pxpEJj = pxpEJ->at(j); + auto& pypEJj = pypEJ->at(j); + auto term1 = -pdistxypEJi * pdistxypEJj; + auto term2 = ppxpEJpEJi->at(j) * x + ppypEJpEJi->at(j) * y; + auto term3 = pxpEJi * pxpEJj + pypEJi * pypEJj; + auto ppdistxypEJpEJij = (term1 + term2 + term3) / distxy; + ppdistxypEJpEJi->atiput(j, ppdistxypEJpEJij); + if (i < j) ppdistxypEJpEJ->atijput(j, i, ppdistxypEJpEJij); + } + } +} + +void MbD::DistxyIeqcJeqc::calc_ppdistxypXIpEJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + auto ppxpXIpEJ = xIeJeIe->ppvaluepXIpEJ(); + auto ppypXIpEJ = yIeJeIe->ppvaluepXIpEJ(); + for (int i = 0; i < 4; i++) + { + auto& ppdistxypXIpEJi = ppdistxypXIpEJ->at(i); + auto& pdistxypXIi = pdistxypXI->at(i); + auto& ppxpXIpEJi = ppxpXIpEJ->at(i); + auto& ppypXIpEJi = ppypXIpEJ->at(i); + auto& pxpXIi = pxpXI->at(i); + auto& pypXIi = pypXI->at(i); + for (int j = 0; j < 4; j++) + { + auto& pdistxypEJj = pdistxypEJ->at(j); + auto& pxpEJj = pxpEJ->at(j); + auto& pypEJj = pypEJ->at(j); + auto term1 = -pdistxypXIi * pdistxypEJj; + auto term2 = ppxpXIpEJi->at(j) * x + ppypXIpEJi->at(j) * y; + auto term3 = pxpXIi * pxpEJj + pypXIi * pypEJj; + auto ppdistxypXIpEJij = (term1 + term2 + term3) / distxy; + ppdistxypXIpEJi->atiput(j, ppdistxypXIpEJij); + } + } +} + +void MbD::DistxyIeqcJeqc::calc_ppdistxypXIpXJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + auto ppxpXIpXJ = xIeJeIe->ppvaluepXIpXJ(); + auto ppypXIpXJ = yIeJeIe->ppvaluepXIpXJ(); + for (int i = 0; i < 4; i++) + { + auto& ppdistxypXIpXJi = ppdistxypXIpXJ->at(i); + auto& pdistxypXIi = pdistxypXI->at(i); + auto& ppxpXIpXJi = ppxpXIpXJ->at(i); + auto& ppypXIpXJi = ppypXIpXJ->at(i); + auto& pxpXIi = pxpXI->at(i); + auto& pypXIi = pypXI->at(i); + for (int j = 0; j < 4; j++) + { + auto& pdistxypXJj = pdistxypXJ->at(j); + auto& pxpXJj = pxpXJ->at(j); + auto& pypXJj = pypXJ->at(j); + auto term1 = -pdistxypXIi * pdistxypXJj; + auto term2 = ppxpXIpXJi->at(j) * x + ppypXIpXJi->at(j) * y; + auto term3 = pxpXIi * pxpXJj + pypXIi * pypXJj; + auto ppdistxypXIpXJij = (term1 + term2 + term3) / distxy; + ppdistxypXIpXJi->atiput(j, ppdistxypXIpXJij); + } + } +} + +void MbD::DistxyIeqcJeqc::calc_ppdistxypXJpEJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + auto ppxpXJpEJ = xIeJeIe->ppvaluepXJpEJ(); + auto ppypXJpEJ = yIeJeIe->ppvaluepXJpEJ(); + for (int i = 0; i < 4; i++) + { + auto& ppdistxypXJpEJi = ppdistxypXJpEJ->at(i); + auto& pdistxypXJi = pdistxypXJ->at(i); + auto& ppxpXJpEJi = ppxpXJpEJ->at(i); + auto& ppypXJpEJi = ppypXJpEJ->at(i); + auto& pxpXJi = pxpXJ->at(i); + auto& pypXJi = pypXJ->at(i); + for (int j = 0; j < 4; j++) + { + auto& pdistxypEJj = pdistxypEJ->at(j); + auto& pxpEJj = pxpEJ->at(j); + auto& pypEJj = pypEJ->at(j); + auto term1 = -pdistxypXJi * pdistxypEJj; + auto term2 = ppxpXJpEJi->at(j) * x + ppypXJpEJi->at(j) * y; + auto term3 = pxpXJi * pxpEJj + pypXJi * pypEJj; + auto ppdistxypXJpEJij = (term1 + term2 + term3) / distxy; + ppdistxypXJpEJi->atiput(j, ppdistxypXJpEJij); + } + } +} + +void MbD::DistxyIeqcJeqc::calc_ppdistxypXJpXJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + auto ppxpXJpXJ = xIeJeIe->ppvaluepXJpXJ(); + auto ppypXJpXJ = yIeJeIe->ppvaluepXJpXJ(); + for (int i = 0; i < 3; i++) + { + auto& ppdistxypXJpXJi = ppdistxypXJpXJ->at(i); + auto& pdistxypXJi = pdistxypXJ->at(i); + auto& ppxpXJpXJi = ppxpXJpXJ->at(i); + auto& ppypXJpXJi = ppypXJpXJ->at(i); + auto& pxpXJi = pxpXJ->at(i); + auto& pypXJi = pypXJ->at(i); + for (int j = i; j < 3; j++) + { + auto& pdistxypXJj = pdistxypXJ->at(j); + auto& pxpXJj = pxpXJ->at(j); + auto& pypXJj = pypXJ->at(j); + auto term1 = -pdistxypXJi * pdistxypXJj; + auto term2 = ppxpXJpXJi->at(j) * x + ppypXJpXJi->at(j) * y; + auto term3 = pxpXJi * pxpXJj + pypXJi * pypXJj; + auto ppdistxypXJpXJij = (term1 + term2 + term3) / distxy; + ppdistxypXJpXJi->atiput(j, ppdistxypXJpXJij); + if (i < j) ppdistxypXJpXJ->atijput(j, i, ppdistxypXJpXJij); + } + } +} + +void MbD::DistxyIeqcJeqc::calc_pdistxypEJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + for (int i = 0; i < 4; i++) + { + auto term = pxpEJ->at(i) * x + pypEJ->at(i) * y; + pdistxypEJ->atiput(i, term / distxy); + } +} + +void MbD::DistxyIeqcJeqc::calc_pdistxypXJ() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + for (int i = 0; i < 3; i++) + { + auto term = pxpXJ->at(i) * x + pypXJ->at(i) * y; + pdistxypXJ->atiput(i, term / distxy); + } +} + +void MbD::DistxyIeqcJeqc::calcPostDynCorrectorIteration() +{ + DistxyIeqcJec::calcPostDynCorrectorIteration(); + this->calc_pdistxypXJ(); + this->calc_pdistxypEJ(); + this->calc_ppdistxypXIpXJ(); + this->calc_ppdistxypXIpEJ(); + this->calc_ppdistxypEIpXJ(); + this->calc_ppdistxypEIpEJ(); + this->calc_ppdistxypXJpXJ(); + this->calc_ppdistxypXJpEJ(); + this->calc_ppdistxypEJpEJ(); +} + +void MbD::DistxyIeqcJeqc::init_xyIeJeIe() +{ + xIeJeIe = CREATE::With(frmI, frmJ, 0); + yIeJeIe = CREATE::With(frmI, frmJ, 1); +} + +void MbD::DistxyIeqcJeqc::initialize() +{ + DistxyIeqcJec::initialize(); + pdistxypXJ = std::make_shared>(3); + pdistxypEJ = std::make_shared>(4); + ppdistxypXIpXJ = std::make_shared>(3, 3); + ppdistxypXIpEJ = std::make_shared>(3, 4); + ppdistxypEIpXJ = std::make_shared>(4, 3); + ppdistxypEIpEJ = std::make_shared>(4, 4); + ppdistxypXJpXJ = std::make_shared>(3, 3); + ppdistxypXJpEJ = std::make_shared>(3, 4); + ppdistxypEJpEJ = std::make_shared>(4, 4); +} + +FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEIpEJ() +{ + return ppdistxypEIpEJ; +} + +FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEIpXJ() +{ + return ppdistxypEIpXJ; +} + +FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEJpEJ() +{ + return ppdistxypEJpEJ; +} + +FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepXIpEJ() +{ + return ppdistxypXIpEJ; +} + +FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepXIpXJ() +{ + return ppdistxypXIpXJ; +} + +FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepXJpEJ() +{ + return ppdistxypXJpEJ; +} + +FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepXJpXJ() +{ + return ppdistxypXJpXJ; +} + +FRowDsptr MbD::DistxyIeqcJeqc::pvaluepEJ() +{ + return pdistxypEJ; +} + +FRowDsptr MbD::DistxyIeqcJeqc::pvaluepXJ() +{ + return pdistxypXJ; +} diff --git a/MbDCode/DistxyIeqcJeqc.h b/MbDCode/DistxyIeqcJeqc.h new file mode 100644 index 0000000..d434753 --- /dev/null +++ b/MbDCode/DistxyIeqcJeqc.h @@ -0,0 +1,41 @@ +#pragma once + +#include "DistxyIeqcJec.h" + +namespace MbD { + class DistxyIeqcJeqc : public DistxyIeqcJec + { + //pdistxypXJ pdistxypEJ ppdistxypXIpXJ ppdistxypXIpEJ ppdistxypEIpXJ ppdistxypEIpEJ ppdistxypXJpXJ ppdistxypXJpEJ ppdistxypEJpEJ + public: + DistxyIeqcJeqc(); + DistxyIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calc_ppdistxypEIpEJ(); + void calc_ppdistxypEIpXJ(); + void calc_ppdistxypEJpEJ(); + void calc_ppdistxypXIpEJ(); + void calc_ppdistxypXIpXJ(); + void calc_ppdistxypXJpEJ(); + void calc_ppdistxypXJpXJ(); + void calc_pdistxypEJ(); + void calc_pdistxypXJ(); + void calcPostDynCorrectorIteration() override; + void init_xyIeJeIe() override; + void initialize() override; + FMatDsptr ppvaluepEIpEJ() override; + FMatDsptr ppvaluepEIpXJ() override; + FMatDsptr ppvaluepEJpEJ() override; + FMatDsptr ppvaluepXIpEJ() override; + FMatDsptr ppvaluepXIpXJ() override; + FMatDsptr ppvaluepXJpEJ() override; + FMatDsptr ppvaluepXJpXJ() override; + FRowDsptr pvaluepEJ() override; + FRowDsptr pvaluepXJ() override; + + + FRowDsptr pdistxypXJ, pdistxypEJ; + FMatDsptr ppdistxypXIpXJ, ppdistxypXIpEJ, ppdistxypEIpXJ, ppdistxypEIpEJ, ppdistxypXJpXJ, ppdistxypXJpEJ, ppdistxypEJpEJ; + + }; +} + diff --git a/MbDCode/DistxyIeqctJeqc.cpp b/MbDCode/DistxyIeqctJeqc.cpp new file mode 100644 index 0000000..4a9012c --- /dev/null +++ b/MbDCode/DistxyIeqctJeqc.cpp @@ -0,0 +1,3 @@ +#include "DistxyIeqctJeqc.h" + +using namespace MbD; diff --git a/MbDCode/DistxyIeqctJeqc.h b/MbDCode/DistxyIeqctJeqc.h new file mode 100644 index 0000000..e2e5430 --- /dev/null +++ b/MbDCode/DistxyIeqctJeqc.h @@ -0,0 +1,15 @@ +#pragma once + +#include "DistxyIeqcJeqc.h" + +namespace MbD { + class DistxyIeqctJeqc : + public DistxyIeqcJeqc + { + //pdistxypt ppdistxypXIpt ppdistxypEIpt ppdistxypXJpt ppdistxypEJpt ppdistxyptpt + public: + //ToDo: add member functions. + + }; +} + diff --git a/MbDCode/EndFramec.cpp b/MbDCode/EndFramec.cpp index 2131fcb..fd50beb 100644 --- a/MbDCode/EndFramec.cpp +++ b/MbDCode/EndFramec.cpp @@ -3,6 +3,8 @@ #include "EndFramec.h" #include "MarkerFrame.h" +#include "EndFrameqc.h" +#include "CREATE.h" using namespace MbD; @@ -80,3 +82,10 @@ FMatDsptr EndFramec::aBOp() { return FMatDsptr(); } + +std::shared_ptr MbD::EndFramec::newCopyEndFrameqc() +{ + auto frmIeqc = CREATE::With(); + markerFrame->addEndFrame(frmIeqc); + return frmIeqc; +} diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h index cc89fb4..014c60f 100644 --- a/MbDCode/EndFramec.h +++ b/MbDCode/EndFramec.h @@ -8,10 +8,7 @@ namespace MbD { class MarkerFrame; - //template - //class FullColumn; - //template - //class FullMatrix; + class EndFrameqc; class EndFramec : public CartesianFrame { @@ -34,6 +31,7 @@ namespace MbD { virtual FColDsptr rpep(); virtual FColFMatDsptr pAOppE(); virtual FMatDsptr aBOp(); + std::shared_ptr newCopyEndFrameqc(); MarkerFrame* markerFrame; //Use raw pointer when pointing backwards. FColDsptr rOeO = std::make_shared>(3); diff --git a/MbDCode/EndFrameqct.h b/MbDCode/EndFrameqct.h index 2349033..79f2e8b 100644 --- a/MbDCode/EndFrameqct.h +++ b/MbDCode/EndFrameqct.h @@ -1,4 +1,5 @@ #pragma once + #include "EndFrameqc.h" #include "Symbolic.h" diff --git a/MbDCode/EulerArray.h b/MbDCode/EulerArray.h index cd1c589..85defbf 100644 --- a/MbDCode/EulerArray.h +++ b/MbDCode/EulerArray.h @@ -1,4 +1,5 @@ #pragma once + #include "FullColumn.h" namespace MbD { diff --git a/MbDCode/EulerConstraint.h b/MbDCode/EulerConstraint.h index 034eb32..8a21a00 100644 --- a/MbDCode/EulerConstraint.h +++ b/MbDCode/EulerConstraint.h @@ -1,4 +1,5 @@ #pragma once + #include #include diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h index 0cb7e27..fb56a27 100644 --- a/MbDCode/EulerParameters.h +++ b/MbDCode/EulerParameters.h @@ -1,4 +1,5 @@ #pragma once + #include "EulerArray.h" #include "FullColumn.h" #include "FullMatrix.h" diff --git a/MbDCode/FixedJoint.cpp b/MbDCode/FixedJoint.cpp new file mode 100644 index 0000000..e895129 --- /dev/null +++ b/MbDCode/FixedJoint.cpp @@ -0,0 +1,28 @@ +#include "FixedJoint.h" +#include "System.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::FixedJoint::FixedJoint() +{ +} + +MbD::FixedJoint::FixedJoint(const char* str) : AtPointJoint(str) +{ +} + +void MbD::FixedJoint::initializeGlobally() +{ + if (constraints->empty()) + { + createAtPointConstraints(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/FixedJoint.h b/MbDCode/FixedJoint.h new file mode 100644 index 0000000..6295cd0 --- /dev/null +++ b/MbDCode/FixedJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "AtPointJoint.h" + +namespace MbD { + class FixedJoint : public AtPointJoint + { + // + public: + FixedJoint(); + FixedJoint(const char* str); + void initializeGlobally() override; + + + }; +} diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h index 947ce84..87a3f3e 100644 --- a/MbDCode/FullColumn.h +++ b/MbDCode/FullColumn.h @@ -1,4 +1,5 @@ #pragma once + #include #include @@ -13,6 +14,7 @@ namespace MbD { class FullColumn : public FullVector { public: + FullColumn() : FullVector() {} FullColumn(std::vector vec) : FullVector(vec) {} FullColumn(int count) : FullVector(count) {} FullColumn(int count, const T& value) : FullVector(count, value) {} diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h index a5d58a5..89e6400 100644 --- a/MbDCode/FullMatrix.h +++ b/MbDCode/FullMatrix.h @@ -1,4 +1,5 @@ #pragma once + #include #include "RowTypeMatrix.h" @@ -45,6 +46,7 @@ namespace MbD { std::shared_ptr> times(double a); std::shared_ptr> transposeTimesFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> plusFullMatrix(std::shared_ptr> fullMat); + std::shared_ptr> minusFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> transpose(); std::shared_ptr> negated(); void symLowerWithUpper(); @@ -125,6 +127,16 @@ namespace MbD { return answer; } template + inline std::shared_ptr> FullMatrix::minusFullMatrix(std::shared_ptr> fullMat) + { + int n = (int)this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) { + answer->at(i) = this->at(i)->minusFullRow(fullMat->at(i)); + } + return answer; + } + template inline std::shared_ptr> FullMatrix::transpose() { int nrow = this->nrow(); diff --git a/MbDCode/FullMotion.cpp b/MbDCode/FullMotion.cpp new file mode 100644 index 0000000..f71cb0c --- /dev/null +++ b/MbDCode/FullMotion.cpp @@ -0,0 +1,34 @@ +#include "FullMotion.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::FullMotion::FullMotion() +{ +} + +MbD::FullMotion::FullMotion(const char* str) +{ +} + +void MbD::FullMotion::initializeGlobally() +{ + if (constraints->empty()) { + initMotions(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + PrescribedMotion::initializeGlobally(); + } +} + +void MbD::FullMotion::initMotions() +{ +} diff --git a/MbDCode/FullMotion.h b/MbDCode/FullMotion.h new file mode 100644 index 0000000..3a2d941 --- /dev/null +++ b/MbDCode/FullMotion.h @@ -0,0 +1,17 @@ +#pragma once + +#include "PrescribedMotion.h" + +namespace MbD { + class FullMotion : public PrescribedMotion + { + //frIJI fangIJJ + public: + FullMotion(); + FullMotion(const char* str); + void initializeGlobally() override; + void initMotions() override; + + }; +} + diff --git a/MbDCode/FullRow.h b/MbDCode/FullRow.h index 3542e39..f9cdc5b 100644 --- a/MbDCode/FullRow.h +++ b/MbDCode/FullRow.h @@ -1,4 +1,5 @@ #pragma once + #include "FullVector.h" //#include "FullColumn.h" @@ -12,6 +13,7 @@ namespace MbD { class FullRow : public FullVector { public: + FullRow() : FullVector() {} FullRow(std::vector vec) : FullVector(vec) {} FullRow(int count) : FullVector(count) {} FullRow(int count, const T& value) : FullVector(count, value) {} @@ -26,6 +28,7 @@ namespace MbD { std::shared_ptr> timesFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> timesTransposeFullMatrix(std::shared_ptr> fullMat); void equalSelfPlusFullRowTimes(std::shared_ptr> fullRow, double factor); + void equalFullRow(std::shared_ptr> fullRow); std::shared_ptr> transpose(); std::shared_ptr> copy(); void atiplusFullRow(int j, std::shared_ptr> fullRow); @@ -101,6 +104,11 @@ namespace MbD { this->equalSelfPlusFullVectortimes(fullRow, factor); } template + inline void FullRow::equalFullRow(std::shared_ptr> fullRow) + { + this->equalArrayAt(fullRow, 0); + } + template inline std::shared_ptr> FullRow::transpose() { return std::make_shared>(*this); diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index 8904205..9b4e7c3 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -1,4 +1,5 @@ #pragma once + #include #include "Array.h" @@ -8,7 +9,7 @@ namespace MbD { class FullVector : public Array { public: - FullVector() {} + FullVector() : Array() {} FullVector(std::vector vec) : Array(vec) {} FullVector(int count) : Array(count) {} FullVector(int count, const T& value) : Array(count, value) {} diff --git a/MbDCode/GearConstraintIJ.cpp b/MbDCode/GearConstraintIJ.cpp index 9956005..47438e1 100644 --- a/MbDCode/GearConstraintIJ.cpp +++ b/MbDCode/GearConstraintIJ.cpp @@ -1 +1,83 @@ #include "GearConstraintIJ.h" + +using namespace MbD; + +MbD::GearConstraintIJ::GearConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : ConstraintIJ(frmi, frmj) +{ +} + +void MbD::GearConstraintIJ::calcPostDynCorrectorIteration() +{ + aG = orbitJeIe->value() + (this->ratio() * orbitIeJe->value()) - aConstant; +} + +void MbD::GearConstraintIJ::initialize() +{ + ConstraintIJ::initialize(); + this->initorbitsIJ(); +} + +void MbD::GearConstraintIJ::initializeGlobally() +{ + orbitIeJe->initializeGlobally(); + orbitJeIe->initializeGlobally(); +} + +void MbD::GearConstraintIJ::initializeLocally() +{ + orbitIeJe->initializeLocally(); + orbitJeIe->initializeLocally(); +} + +void MbD::GearConstraintIJ::initorbitsIJ() +{ + assert(false); +} + +void MbD::GearConstraintIJ::postInput() +{ + orbitIeJe->postInput(); + orbitJeIe->postInput(); + aConstant = orbitJeIe->value() + (this->ratio() * orbitIeJe->value()); + ConstraintIJ::postInput(); +} + +void MbD::GearConstraintIJ::postPosICIteration() +{ + orbitIeJe->postPosICIteration(); + orbitJeIe->postPosICIteration(); + ConstraintIJ::postPosICIteration(); +} + +void MbD::GearConstraintIJ::preAccIC() +{ + orbitIeJe->preAccIC(); + orbitJeIe->preAccIC(); + ConstraintIJ::preAccIC(); +} + +void MbD::GearConstraintIJ::prePosIC() +{ + orbitIeJe->prePosIC(); + orbitJeIe->prePosIC(); + ConstraintIJ::prePosIC(); +} + +void MbD::GearConstraintIJ::preVelIC() +{ + orbitIeJe->preVelIC(); + orbitJeIe->preVelIC(); + ConstraintIJ::preVelIC(); +} + +double MbD::GearConstraintIJ::ratio() +{ + return radiusI / radiusJ; +} + +void MbD::GearConstraintIJ::simUpdateAll() +{ + orbitIeJe->simUpdateAll(); + orbitJeIe->simUpdateAll(); + ConstraintIJ::simUpdateAll(); +} diff --git a/MbDCode/GearConstraintIJ.h b/MbDCode/GearConstraintIJ.h index a969444..37e405c 100644 --- a/MbDCode/GearConstraintIJ.h +++ b/MbDCode/GearConstraintIJ.h @@ -1,14 +1,30 @@ #pragma once #include "ConstraintIJ.h" +#include "OrbitAngleZIecJec.h" namespace MbD { - class GearConstraintIJ : public ConstraintIJ - { - //orbitIeJe orbitJeIe radiusI radiusJ - public: + class GearConstraintIJ : public ConstraintIJ + { + //orbitIeJe orbitJeIe radiusI radiusJ + public: + GearConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); + void calcPostDynCorrectorIteration() override; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + virtual void initorbitsIJ(); + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + double ratio(); + void simUpdateAll() override; - }; + std::shared_ptr orbitIeJe, orbitJeIe; + double radiusI, radiusJ; + }; } diff --git a/MbDCode/GearConstraintIqcJc.cpp b/MbDCode/GearConstraintIqcJc.cpp new file mode 100644 index 0000000..dbff7d1 --- /dev/null +++ b/MbDCode/GearConstraintIqcJc.cpp @@ -0,0 +1,130 @@ +#include "GearConstraintIqcJc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "OrbitAngleZIeqcJec.h" + +using namespace MbD; + +MbD::GearConstraintIqcJc::GearConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : GearConstraintIJ(frmi, frmj) +{ +} + +void MbD::GearConstraintIqcJc::addToJointForceI(FColDsptr col) +{ + col->equalSelfPlusFullVectortimes(pGpXI, lam); +} + +void MbD::GearConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto cForceT = pGpXI->times(lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto rIpIeIp = frmIeqc->rpep(); + auto pAOIppEI = frmIeqc->pAOppE(); + auto aBOIp = frmIeqc->aBOp(); + auto fpAOIppEIrIpIeIp = std::make_shared>(4, 0.0); + for (int i = 0; i < 4; i++) + { + auto dum = cForceT->timesFullColumn(pAOIppEI->at(i)->timesFullColumn(rIpIeIp)); + fpAOIppEIrIpIeIp->atiput(i, dum); + } + auto lampGpE = pGpEI->transpose()->times(lam); + auto c2Torque = aBOIp->timesFullColumn(lampGpE->minusFullColumn(fpAOIppEIrIpIeIp)); + jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); +} + +void MbD::GearConstraintIqcJc::calc_pGpEI() +{ + pGpEI = orbitJeIe->pvaluepEJ()->plusFullRow(orbitIeJe->pvaluepEI()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJc::calc_pGpXI() +{ + pGpXI = orbitJeIe->pvaluepXJ()->plusFullRow(orbitIeJe->pvaluepXI()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJc::calc_ppGpEIpEI() +{ + ppGpEIpEI = orbitJeIe->ppvaluepEJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepEIpEI()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJc::calc_ppGpXIpEI() +{ + ppGpXIpEI = orbitJeIe->ppvaluepXJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpEI()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJc::calc_ppGpXIpXI() +{ + ppGpXIpXI = orbitJeIe->ppvaluepXJpXJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJc::calcPostDynCorrectorIteration() +{ + GearConstraintIJ::calcPostDynCorrectorIteration(); + this->calc_pGpXI(); + this->calc_pGpEI(); + this->calc_ppGpXIpXI(); + this->calc_ppGpXIpEI(); + this->calc_ppGpEIpEI(); +} + +void MbD::GearConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpXI->timesFullColumn(efrmIqc->qXddot()); + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += qXdotI->transposeTimesFullColumn(ppGpXIpXI->timesFullColumn(qXdotI)); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEI->timesFullColumn(qEdotI))); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} + +void MbD::GearConstraintIqcJc::fillPosICError(FColDsptr col) +{ + GearConstraintIJ::fillPosICError(col); + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::GearConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullMatrixtimes(iqXI, iqXI, ppGpXIpXI, lam); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::GearConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); +} + +void MbD::GearConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::GearConstraintIqcJc::initorbitsIJ() +{ + orbitIeJe = CREATE::With(frmI, frmJ); + orbitJeIe = CREATE::With(frmJ, frmI); +} + +void MbD::GearConstraintIqcJc::useEquationNumbers() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + iqXI = frmIeqc->iqX(); + iqEI = frmIeqc->iqE(); +} diff --git a/MbDCode/GearConstraintIqcJc.h b/MbDCode/GearConstraintIqcJc.h new file mode 100644 index 0000000..bb9746d --- /dev/null +++ b/MbDCode/GearConstraintIqcJc.h @@ -0,0 +1,33 @@ +#pragma once + +#include "GearConstraintIJ.h" + +namespace MbD { + class GearConstraintIqcJc : public GearConstraintIJ + { + //pGpXI pGpEI ppGpXIpXI ppGpXIpEI ppGpEIpEI iqXI iqEI + public: + GearConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj); + + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; + void calc_pGpEI(); + void calc_pGpXI(); + void calc_ppGpEIpEI(); + void calc_ppGpXIpEI(); + void calc_ppGpXIpXI(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void initorbitsIJ() override; + void useEquationNumbers() override; + + FRowDsptr pGpXI, pGpEI; + FMatDsptr ppGpXIpXI, ppGpXIpEI, ppGpEIpEI; + int iqXI, iqEI; + }; +} + diff --git a/MbDCode/GearConstraintIqcJqc.cpp b/MbDCode/GearConstraintIqcJqc.cpp new file mode 100644 index 0000000..01ea39c --- /dev/null +++ b/MbDCode/GearConstraintIqcJqc.cpp @@ -0,0 +1,156 @@ +#include "GearConstraintIqcJqc.h" +#include "EndFrameqc.h" +#include "OrbitAngleZIeqcJeqc.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::GearConstraintIqcJqc::GearConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj) : GearConstraintIqcJc(frmi, frmj) +{ +} + +void MbD::GearConstraintIqcJqc::calc_pGpEJ() +{ + pGpEJ = orbitJeIe->pvaluepEI()->plusFullRow(orbitIeJe->pvaluepEJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_pGpXJ() +{ + pGpXJ = orbitJeIe->pvaluepXI()->plusFullRow(orbitIeJe->pvaluepXJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_ppGpEIpEJ() +{ + ppGpEIpEJ = orbitJeIe->ppvaluepEIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpEJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_ppGpEIpXJ() +{ + ppGpEIpXJ = orbitJeIe->ppvaluepXIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpXJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_ppGpEJpEJ() +{ + ppGpEJpEJ = orbitJeIe->ppvaluepEIpEI()->plusFullMatrix(orbitIeJe->ppvaluepEJpEJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_ppGpXIpEJ() +{ + ppGpXIpEJ = orbitJeIe->ppvaluepEIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpEJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_ppGpXIpXJ() +{ + ppGpXIpXJ = orbitJeIe->ppvaluepXIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpXJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_ppGpXJpEJ() +{ + ppGpXJpEJ = orbitJeIe->ppvaluepXIpEI()->plusFullMatrix(orbitIeJe->ppvaluepXJpEJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calc_ppGpXJpXJ() +{ + ppGpXJpXJ = orbitJeIe->ppvaluepXIpXI()->plusFullMatrix(orbitIeJe->ppvaluepXJpXJ()->times(this->ratio())); +} + +void MbD::GearConstraintIqcJqc::calcPostDynCorrectorIteration() +{ + GearConstraintIqcJc::calcPostDynCorrectorIteration(); + this->calc_pGpXJ(); + this->calc_pGpEJ(); + this->calc_ppGpXIpXJ(); + this->calc_ppGpXIpEJ(); + this->calc_ppGpEIpXJ(); + this->calc_ppGpEIpEJ(); + this->calc_ppGpXJpXJ(); + this->calc_ppGpXJpEJ(); + this->calc_ppGpEJpEJ(); +} + +void MbD::GearConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + GearConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto frmJeqc = std::static_pointer_cast(frmJ); + auto qXdotI = frmIeqc->qXdot(); + auto qEdotI = frmIeqc->qEdot(); + auto qXdotJ = frmJeqc->qXdot(); + auto qEdotJ = frmJeqc->qEdot(); + double sum = 0.0; + sum += pGpXJ->timesFullColumn(frmJeqc->qXddot()); + sum += pGpEJ->timesFullColumn(frmJeqc->qEddot()); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpXJ->timesFullColumn(qXdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpXJ->timesFullColumn(qXdotJ))); + sum += qXdotJ->transposeTimesFullColumn(ppGpXJpXJ->timesFullColumn(qXdotJ)); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEJ->timesFullColumn(qEdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))); + sum += 2.0 * (qXdotJ->transposeTimesFullColumn(ppGpXJpEJ->timesFullColumn(qEdotJ))); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} + +void MbD::GearConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + GearConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::GearConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + GearConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpXIpXJlam = ppGpXIpXJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqXJ, ppGpXIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqXI, ppGpXIpXJlam); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + mat->atijplusFullMatrixtimes(iqXJ, iqXJ, ppGpXJpXJ, lam); + auto ppGpXIpEJlam = ppGpXIpEJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqEJ, ppGpXIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXI, ppGpXIpEJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + auto ppGpXJpEJlam = ppGpXJpEJ->times(lam); + mat->atijplusFullMatrix(iqXJ, iqEJ, ppGpXJpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXJ, ppGpXJpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::GearConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + GearConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} + +void MbD::GearConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + GearConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::GearConstraintIqcJqc::initorbitsIJ() +{ + orbitIeJe = CREATE::With(frmI, frmJ); + orbitJeIe = CREATE::With(frmJ, frmI); +} + +void MbD::GearConstraintIqcJqc::useEquationNumbers() +{ + GearConstraintIqcJc::useEquationNumbers(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqXJ = frmJeqc->iqX(); + iqEJ = frmJeqc->iqE(); +} diff --git a/MbDCode/GearConstraintIqcJqc.h b/MbDCode/GearConstraintIqcJqc.h new file mode 100644 index 0000000..667133a --- /dev/null +++ b/MbDCode/GearConstraintIqcJqc.h @@ -0,0 +1,36 @@ +#pragma once + +#include "GearConstraintIqcJc.h" + +namespace MbD { + class GearConstraintIqcJqc : public GearConstraintIqcJc + { + //pGpXJ pGpEJ ppGpXIpXJ ppGpXIpEJ ppGpEIpXJ ppGpEIpEJ ppGpXJpXJ ppGpXJpEJ ppGpEJpEJ iqXJ iqEJ + public: + GearConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calc_pGpEJ(); + void calc_pGpXJ(); + void calc_ppGpEIpEJ(); + void calc_ppGpEIpXJ(); + void calc_ppGpEJpEJ(); + void calc_ppGpXIpEJ(); + void calc_ppGpXIpXJ(); + void calc_ppGpXJpEJ(); + void calc_ppGpXJpXJ(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void initorbitsIJ() override; + void useEquationNumbers() override; + + FRowDsptr pGpXJ, pGpEJ; + FMatDsptr ppGpXIpXJ, ppGpXIpEJ, ppGpEIpXJ, ppGpEIpEJ, ppGpXJpXJ, ppGpXJpEJ, ppGpEJpEJ; + int iqXJ, iqEJ; + + }; +} + diff --git a/MbDCode/GearJoint.cpp b/MbDCode/GearJoint.cpp new file mode 100644 index 0000000..ab9867f --- /dev/null +++ b/MbDCode/GearJoint.cpp @@ -0,0 +1,30 @@ +#include "GearJoint.h" +#include "CREATE.h" +#include "GearConstraintIJ.h" +#include "System.h" + +using namespace MbD; + +MbD::GearJoint::GearJoint() +{ +} + +MbD::GearJoint::GearJoint(const char* str) +{ +} + +void MbD::GearJoint::initializeGlobally() +{ + if (constraints->empty()) + { + auto gearIJ = CREATE::With(frmI, frmJ); + gearIJ->radiusI = radiusI; + gearIJ->radiusJ = radiusJ; + gearIJ->setConstant(aConstant); + addConstraint(gearIJ); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/GearJoint.h b/MbDCode/GearJoint.h new file mode 100644 index 0000000..ee76a28 --- /dev/null +++ b/MbDCode/GearJoint.h @@ -0,0 +1,17 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class GearJoint : public Joint + { + //radiusI radiusJ aConstant + public: + GearJoint(); + GearJoint(const char* str); + void initializeGlobally() override; + + double radiusI, radiusJ, aConstant; + }; +} + diff --git a/MbDCode/ICKineIntegrator.h b/MbDCode/ICKineIntegrator.h index f7c1c4b..ab4a2db 100644 --- a/MbDCode/ICKineIntegrator.h +++ b/MbDCode/ICKineIntegrator.h @@ -1,4 +1,5 @@ #pragma once + #include "QuasiIntegrator.h" namespace MbD { diff --git a/MbDCode/InLineJoint.cpp b/MbDCode/InLineJoint.cpp new file mode 100644 index 0000000..4bc29db --- /dev/null +++ b/MbDCode/InLineJoint.cpp @@ -0,0 +1,16 @@ +#include "InLineJoint.h" +#include "CREATE.h" + +MbD::InLineJoint::InLineJoint() +{ +} + +MbD::InLineJoint::InLineJoint(const char* str) +{ +} + +void MbD::InLineJoint::createInLineConstraints() +{ + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); +} diff --git a/MbDCode/InLineJoint.h b/MbDCode/InLineJoint.h new file mode 100644 index 0000000..adb0f6b --- /dev/null +++ b/MbDCode/InLineJoint.h @@ -0,0 +1,17 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class InLineJoint : public Joint + { + // + public: + InLineJoint(); + InLineJoint(const char* str); + + void createInLineConstraints(); + + }; +} + diff --git a/MbDCode/InPlaneJoint.cpp b/MbDCode/InPlaneJoint.cpp new file mode 100644 index 0000000..171516e --- /dev/null +++ b/MbDCode/InPlaneJoint.cpp @@ -0,0 +1,17 @@ +#include "InPlaneJoint.h" +#include "CREATE.h" + +MbD::InPlaneJoint::InPlaneJoint() +{ +} + +MbD::InPlaneJoint::InPlaneJoint(const char* str) +{ +} + +void MbD::InPlaneJoint::createInPlaneConstraint() +{ + auto tranCon = CREATE::ConstraintWith(frmI, frmJ, 0); + tranCon->setConstant(offset); + addConstraint(tranCon); +} diff --git a/MbDCode/InPlaneJoint.h b/MbDCode/InPlaneJoint.h new file mode 100644 index 0000000..d3d2443 --- /dev/null +++ b/MbDCode/InPlaneJoint.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class InPlaneJoint : public Joint + { + //offset + public: + InPlaneJoint(); + InPlaneJoint(const char* str); + + void createInPlaneConstraint(); + + double offset; + }; +} + diff --git a/MbDCode/Integrator.h b/MbDCode/Integrator.h index d96b40b..99893c4 100644 --- a/MbDCode/Integrator.h +++ b/MbDCode/Integrator.h @@ -1,4 +1,5 @@ #pragma once + #include #include "Solver.h" diff --git a/MbDCode/Item.h b/MbDCode/Item.h index c502282..bfd0476 100644 --- a/MbDCode/Item.h +++ b/MbDCode/Item.h @@ -1,4 +1,5 @@ #pragma once + #include #include diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h index 1d912b9..6af27bf 100644 --- a/MbDCode/Joint.h +++ b/MbDCode/Joint.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/KinematicIeJe.cpp b/MbDCode/KinematicIeJe.cpp index f8f9215..e4fb071 100644 --- a/MbDCode/KinematicIeJe.cpp +++ b/MbDCode/KinematicIeJe.cpp @@ -15,6 +15,156 @@ bool MbD::KinematicIeJe::isKineIJ() return true; } +void MbD::KinematicIeJe::calc_pvaluepXI() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_pvaluepEI() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXIpXI() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXIpEI() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEIpEI() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_pvaluepXJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_pvaluepEJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXIpXJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXIpEJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEIpXJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEIpEJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXJpXJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXJpEJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEJpEJ() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_pvaluepXK() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_pvaluepEK() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXIpEK() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEIpEK() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXJpEK() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEJpEK() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEKpEK() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_pvaluept() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvalueptpt() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_value() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXIpt() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEIpt() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXJpt() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEJpt() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepXKpt() +{ + assert(false); +} + +void MbD::KinematicIeJe::calc_ppvaluepEKpt() +{ + assert(false); +} + FRowDsptr MbD::KinematicIeJe::pvaluepXI() { assert(false); diff --git a/MbDCode/KinematicIeJe.h b/MbDCode/KinematicIeJe.h index ce8718b..7fcab0e 100644 --- a/MbDCode/KinematicIeJe.h +++ b/MbDCode/KinematicIeJe.h @@ -12,6 +12,37 @@ namespace MbD { KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj); bool isKineIJ() override; + virtual void calc_value(); + virtual void calc_pvaluepXI(); + virtual void calc_pvaluepEI(); + virtual void calc_ppvaluepXIpXI(); + virtual void calc_ppvaluepXIpEI(); + virtual void calc_ppvaluepEIpEI(); + virtual void calc_pvaluepXJ(); + virtual void calc_pvaluepEJ(); + virtual void calc_ppvaluepXIpXJ(); + virtual void calc_ppvaluepXIpEJ(); + virtual void calc_ppvaluepEIpXJ(); + virtual void calc_ppvaluepEIpEJ(); + virtual void calc_ppvaluepXJpXJ(); + virtual void calc_ppvaluepXJpEJ(); + virtual void calc_ppvaluepEJpEJ(); + virtual void calc_pvaluepXK(); + virtual void calc_pvaluepEK(); + virtual void calc_ppvaluepXIpEK(); + virtual void calc_ppvaluepEIpEK(); + virtual void calc_ppvaluepXJpEK(); + virtual void calc_ppvaluepEJpEK(); + virtual void calc_ppvaluepEKpEK(); + virtual void calc_pvaluept(); + virtual void calc_ppvaluepXIpt(); + virtual void calc_ppvaluepEIpt(); + virtual void calc_ppvaluepXJpt(); + virtual void calc_ppvaluepEJpt(); + virtual void calc_ppvaluepXKpt(); + virtual void calc_ppvaluepEKpt(); + virtual void calc_ppvalueptpt(); + virtual FRowDsptr pvaluepXI(); virtual FRowDsptr pvaluepEI(); virtual FMatDsptr ppvaluepXIpXI(); diff --git a/MbDCode/LineInPlaneJoint.cpp b/MbDCode/LineInPlaneJoint.cpp new file mode 100644 index 0000000..1bb052a --- /dev/null +++ b/MbDCode/LineInPlaneJoint.cpp @@ -0,0 +1,26 @@ +#include "LineInPlaneJoint.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::LineInPlaneJoint::LineInPlaneJoint() +{ +} + +MbD::LineInPlaneJoint::LineInPlaneJoint(const char* str) +{ +} + +void MbD::LineInPlaneJoint::initializeGlobally() +{ + if (constraints->empty()) + { + this->createInPlaneConstraint(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 2)); + this->root()->hasChanged = true; + } + else { + InPlaneJoint::initializeGlobally(); + } +} diff --git a/MbDCode/LineInPlaneJoint.h b/MbDCode/LineInPlaneJoint.h new file mode 100644 index 0000000..6c8a5b9 --- /dev/null +++ b/MbDCode/LineInPlaneJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "InPlaneJoint.h" + +namespace MbD { + class LineInPlaneJoint : public InPlaneJoint + { + // + public: + LineInPlaneJoint(); + LineInPlaneJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h index b5e0615..a2c61da 100644 --- a/MbDCode/MarkerFrame.h +++ b/MbDCode/MarkerFrame.h @@ -1,4 +1,5 @@ #pragma once + #include #include diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index 37dbc0d..c85995d 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -5,29 +5,13 @@ *********************************************************************/ #include -#include "System.h" -#include "Constant.h" -#include "FullColumn.h" -#include "FullMatrix.h" -#include "DiagonalMatrix.h" -#include "Part.h" -#include "Joint.h" -#include "CylindricalJoint.h" -#include "RevoluteJoint.h" -#include "ZRotation.h" -#include "PartFrame.h" -#include "MarkerFrame.h" -#include "EndFrameqc.h" -#include "EndFrameqct.h" -#include "Product.h" -#include "Symbolic.h" -#include "SystemSolver.h" -#include "MbDCode.h" -#include "Time.h" -#include "CREATE.h" -#include "GESpMatParPvMarkoFast.h" -#include "GESpMatParPvPrecise.h" +#include +#include + #include "CADSystem.h" +#include "CREATE.h" +#include "GESpMatParPvPrecise.h" +#include "ASMTAssembly.h" using namespace MbD; //using namespace CAD; @@ -35,10 +19,12 @@ void runSpMat(); int main() { + //ASMTAssembly::runFile("C:\\Users\\askoh\\OneDrive\\askoh\\visualworks\\vw8.1\\askoh\\64bit\\CADSM\\current\\asm\\00piston.asmt"); + auto externalSys = std::make_shared(); externalSys->runOndselPiston(); externalSys->runPiston(); - //runSpMat(); + runSpMat(); } void runSpMat() { diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index 1f60cc8..a2ea094 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -138,16 +138,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + @@ -155,6 +180,7 @@ + @@ -192,6 +218,10 @@ + + + + @@ -204,15 +234,20 @@ + + + + + @@ -225,6 +260,8 @@ + + @@ -237,6 +274,7 @@ + @@ -247,13 +285,20 @@ + - - - + + + + + + + + + @@ -261,10 +306,19 @@ + + + + + + + + + @@ -272,6 +326,8 @@ + + @@ -283,10 +339,13 @@ + + + @@ -296,6 +355,7 @@ + @@ -309,16 +369,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + @@ -326,6 +411,7 @@ + @@ -363,6 +449,10 @@ + + + + @@ -376,15 +466,20 @@ + + + + + @@ -397,6 +492,8 @@ + + @@ -409,6 +506,7 @@ + @@ -419,13 +517,20 @@ + - - - + + + + + + + + + @@ -433,11 +538,20 @@ + + + + + + + + + @@ -445,6 +559,8 @@ + + @@ -456,11 +572,14 @@ + + + @@ -470,10 +589,14 @@ + + + + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index 0de51a4..cb64dd4 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -498,13 +498,13 @@ Source Files - + Source Files - + Source Files - + Source Files @@ -522,6 +522,186 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -1016,13 +1196,13 @@ Header Files - + Header Files - + Header Files - + Header Files @@ -1040,10 +1220,193 @@ 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 + + + 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 + + + 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 + + + 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 + + + Header Files + Resource Files + + + \ No newline at end of file diff --git a/MbDCode/NewtonRaphsonError.h b/MbDCode/NewtonRaphsonError.h index 701485b..3df04cd 100644 --- a/MbDCode/NewtonRaphsonError.h +++ b/MbDCode/NewtonRaphsonError.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/NoRotationJoint.cpp b/MbDCode/NoRotationJoint.cpp new file mode 100644 index 0000000..373cd31 --- /dev/null +++ b/MbDCode/NoRotationJoint.cpp @@ -0,0 +1,28 @@ +#include "NoRotationJoint.h" +#include "System.h" +#include "DirectionCosineConstraintIJ.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::NoRotationJoint::NoRotationJoint() +{ +} + +MbD::NoRotationJoint::NoRotationJoint(const char* str) +{ +} + +void MbD::NoRotationJoint::initializeGlobally() +{ + if (constraints->empty()) + { + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/NoRotationJoint.h b/MbDCode/NoRotationJoint.h new file mode 100644 index 0000000..e527979 --- /dev/null +++ b/MbDCode/NoRotationJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class NoRotationJoint : public Joint + { + // + public: + NoRotationJoint(); + NoRotationJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/NotKinematicError.h b/MbDCode/NotKinematicError.h index 31f3ada..fd02b29 100644 --- a/MbDCode/NotKinematicError.h +++ b/MbDCode/NotKinematicError.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/OrbitAnglezIecJec.cpp b/MbDCode/OrbitAnglezIecJec.cpp index 44fcd90..4640d6e 100644 --- a/MbDCode/OrbitAnglezIecJec.cpp +++ b/MbDCode/OrbitAnglezIecJec.cpp @@ -1,11 +1,19 @@ #include -#include "OrbitAnglezIecJec.h" +#include "OrbitAngleZIecJec.h" #include "Numeric.h" using namespace MbD; -void MbD::OrbitAnglezIecJec::calcPostDynCorrectorIteration() +MbD::OrbitAngleZIecJec::OrbitAngleZIecJec() +{ +} + +MbD::OrbitAngleZIecJec::OrbitAngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj) : KinematicIeJe(frmi, frmj) +{ +} + +void MbD::OrbitAngleZIecJec::calcPostDynCorrectorIteration() { auto x = xIeJeIe->value(); auto y = yIeJeIe->value(); @@ -20,25 +28,25 @@ void MbD::OrbitAnglezIecJec::calcPostDynCorrectorIteration() dSqOverSSqSq = diffOfSquares / sumOfSquaresSquared; } -void MbD::OrbitAnglezIecJec::initialize() +void MbD::OrbitAngleZIecJec::initialize() { KinematicIeJe::initialize(); this->init_xyIeJeIe(); } -void MbD::OrbitAnglezIecJec::initializeGlobally() +void MbD::OrbitAngleZIecJec::initializeGlobally() { xIeJeIe->initializeGlobally(); yIeJeIe->initializeGlobally(); } -void MbD::OrbitAnglezIecJec::initializeLocally() +void MbD::OrbitAngleZIecJec::initializeLocally() { xIeJeIe->initializeLocally(); yIeJeIe->initializeLocally(); } -void MbD::OrbitAnglezIecJec::postInput() +void MbD::OrbitAngleZIecJec::postInput() { xIeJeIe->postInput(); yIeJeIe->postInput(); @@ -53,14 +61,14 @@ void MbD::OrbitAnglezIecJec::postInput() KinematicIeJe::postInput(); } -void MbD::OrbitAnglezIecJec::postPosICIteration() +void MbD::OrbitAngleZIecJec::postPosICIteration() { xIeJeIe->postPosICIteration(); yIeJeIe->postPosICIteration(); KinematicIeJe::postPosICIteration(); } -void MbD::OrbitAnglezIecJec::preAccIC() +void MbD::OrbitAngleZIecJec::preAccIC() { if (thez == 0.0) this->prePosIC(); xIeJeIe->preAccIC(); @@ -68,7 +76,7 @@ void MbD::OrbitAnglezIecJec::preAccIC() KinematicIeJe::preAccIC(); } -void MbD::OrbitAnglezIecJec::prePosIC() +void MbD::OrbitAngleZIecJec::prePosIC() { xIeJeIe->prePosIC(); yIeJeIe->prePosIC(); @@ -83,21 +91,21 @@ void MbD::OrbitAnglezIecJec::prePosIC() KinematicIeJe::prePosIC(); } -void MbD::OrbitAnglezIecJec::preVelIC() +void MbD::OrbitAngleZIecJec::preVelIC() { xIeJeIe->preVelIC(); yIeJeIe->preVelIC(); KinematicIeJe::preVelIC(); } -void MbD::OrbitAnglezIecJec::simUpdateAll() +void MbD::OrbitAngleZIecJec::simUpdateAll() { xIeJeIe->simUpdateAll(); yIeJeIe->simUpdateAll(); KinematicIeJe::simUpdateAll(); } -double MbD::OrbitAnglezIecJec::value() +double MbD::OrbitAngleZIecJec::value() { return thez; } diff --git a/MbDCode/OrbitAnglezIecJec.h b/MbDCode/OrbitAnglezIecJec.h index 9759769..768dc8c 100644 --- a/MbDCode/OrbitAnglezIecJec.h +++ b/MbDCode/OrbitAnglezIecJec.h @@ -4,10 +4,13 @@ #include "DispCompIecJecIe.h" namespace MbD { - class OrbitAnglezIecJec : public KinematicIeJe + class OrbitAngleZIecJec : public KinematicIeJe { //thez xIeJeIe yIeJeIe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq public: + OrbitAngleZIecJec(); + OrbitAngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj); + void calcPostDynCorrectorIteration() override; virtual void init_xyIeJeIe() = 0; void initialize() override; diff --git a/MbDCode/OrbitAnglezIeqcJec.cpp b/MbDCode/OrbitAnglezIeqcJec.cpp index ecabaef..08077a5 100644 --- a/MbDCode/OrbitAnglezIeqcJec.cpp +++ b/MbDCode/OrbitAnglezIeqcJec.cpp @@ -1,8 +1,18 @@ -#include "OrbitAnglezIeqcJec.h" +#include "OrbitAngleZIeqcJec.h" +#include "CREATE.h" +#include "DispCompIeqcJecIe.h" using namespace MbD; -void MbD::OrbitAnglezIeqcJec::calc_ppthezpEIpEI() +MbD::OrbitAngleZIeqcJec::OrbitAngleZIeqcJec() +{ +} + +MbD::OrbitAngleZIeqcJec::OrbitAngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : OrbitAngleZIecJec(frmi, frmj) +{ +} + +void MbD::OrbitAngleZIeqcJec::calc_ppthezpEIpEI() { auto pxpEI = xIeJeIe->pvaluepEI(); auto pypEI = yIeJeIe->pvaluepEI(); @@ -29,7 +39,7 @@ void MbD::OrbitAnglezIeqcJec::calc_ppthezpEIpEI() } } -void MbD::OrbitAnglezIeqcJec::calc_ppthezpXIpEI() +void MbD::OrbitAngleZIeqcJec::calc_ppthezpXIpEI() { auto pxpXI = xIeJeIe->pvaluepXI(); auto pypXI = yIeJeIe->pvaluepXI(); @@ -56,7 +66,7 @@ void MbD::OrbitAnglezIeqcJec::calc_ppthezpXIpEI() } } -void MbD::OrbitAnglezIeqcJec::calc_ppthezpXIpXI() +void MbD::OrbitAngleZIeqcJec::calc_ppthezpXIpXI() { //ppxpXIpXI = 0 //ppypXIpXI = 0 @@ -79,7 +89,7 @@ void MbD::OrbitAnglezIeqcJec::calc_ppthezpXIpXI() } } -void MbD::OrbitAnglezIeqcJec::calc_pthezpEI() +void MbD::OrbitAngleZIeqcJec::calc_pthezpEI() { auto pxpEI = xIeJeIe->pvaluepEI(); auto pypEI = yIeJeIe->pvaluepEI(); @@ -89,7 +99,7 @@ void MbD::OrbitAnglezIeqcJec::calc_pthezpEI() } } -void MbD::OrbitAnglezIeqcJec::calc_pthezpXI() +void MbD::OrbitAngleZIeqcJec::calc_pthezpXI() { auto pxpXI = xIeJeIe->pvaluepXI(); auto pypXI = yIeJeIe->pvaluepXI(); @@ -99,9 +109,9 @@ void MbD::OrbitAnglezIeqcJec::calc_pthezpXI() } } -void MbD::OrbitAnglezIeqcJec::calcPostDynCorrectorIteration() +void MbD::OrbitAngleZIeqcJec::calcPostDynCorrectorIteration() { - OrbitAnglezIecJec::calcPostDynCorrectorIteration(); + OrbitAngleZIecJec::calcPostDynCorrectorIteration(); this->calc_pthezpXI(); this->calc_pthezpEI(); this->calc_ppthezpXIpXI(); @@ -109,9 +119,15 @@ void MbD::OrbitAnglezIeqcJec::calcPostDynCorrectorIteration() this->calc_ppthezpEIpEI(); } -void MbD::OrbitAnglezIeqcJec::initialize() +void MbD::OrbitAngleZIeqcJec::init_xyIeJeIe() { - OrbitAnglezIecJec::initialize(); + xIeJeIe = CREATE::With(frmI, frmJ, 0); + yIeJeIe = CREATE::With(frmI, frmJ, 1); +} + +void MbD::OrbitAngleZIeqcJec::initialize() +{ + OrbitAngleZIecJec::initialize(); pthezpXI = std::make_shared>(3); pthezpEI = std::make_shared>(4); ppthezpXIpXI = std::make_shared>(3, 3); @@ -119,27 +135,27 @@ void MbD::OrbitAnglezIeqcJec::initialize() ppthezpEIpEI = std::make_shared>(4, 4); } -FMatDsptr MbD::OrbitAnglezIeqcJec::ppvaluepEIpEI() +FMatDsptr MbD::OrbitAngleZIeqcJec::ppvaluepEIpEI() { return ppthezpEIpEI; } -FMatDsptr MbD::OrbitAnglezIeqcJec::ppvaluepXIpEI() +FMatDsptr MbD::OrbitAngleZIeqcJec::ppvaluepXIpEI() { return ppthezpXIpEI; } -FMatDsptr MbD::OrbitAnglezIeqcJec::ppvaluepXIpXI() +FMatDsptr MbD::OrbitAngleZIeqcJec::ppvaluepXIpXI() { return ppthezpXIpXI; } -FRowDsptr MbD::OrbitAnglezIeqcJec::pvaluepEI() +FRowDsptr MbD::OrbitAngleZIeqcJec::pvaluepEI() { return pthezpEI; } -FRowDsptr MbD::OrbitAnglezIeqcJec::pvaluepXI() +FRowDsptr MbD::OrbitAngleZIeqcJec::pvaluepXI() { return pthezpXI; } diff --git a/MbDCode/OrbitAnglezIeqcJec.h b/MbDCode/OrbitAnglezIeqcJec.h index 7009d9e..3220772 100644 --- a/MbDCode/OrbitAnglezIeqcJec.h +++ b/MbDCode/OrbitAnglezIeqcJec.h @@ -1,18 +1,22 @@ #pragma once -#include "OrbitAnglezIecJec.h" +#include "OrbitAngleZIecJec.h" namespace MbD { - class OrbitAnglezIeqcJec : public OrbitAnglezIecJec + class OrbitAngleZIeqcJec : public OrbitAngleZIecJec { //pthezpXI pthezpEI ppthezpXIpXI ppthezpXIpEI ppthezpEIpEI public: + OrbitAngleZIeqcJec(); + OrbitAngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj); + void calc_ppthezpEIpEI(); void calc_ppthezpXIpEI(); void calc_ppthezpXIpXI(); void calc_pthezpEI(); void calc_pthezpXI(); void calcPostDynCorrectorIteration() override; + void init_xyIeJeIe() override; void initialize() override; FMatDsptr ppvaluepEIpEI() override; FMatDsptr ppvaluepXIpEI() override; diff --git a/MbDCode/OrbitAnglezIeqcJeqc.cpp b/MbDCode/OrbitAnglezIeqcJeqc.cpp index 242c30c..88e59c5 100644 --- a/MbDCode/OrbitAnglezIeqcJeqc.cpp +++ b/MbDCode/OrbitAnglezIeqcJeqc.cpp @@ -1,8 +1,18 @@ -#include "OrbitAnglezIeqcJeqc.h" +#include "OrbitAngleZIeqcJeqc.h" +#include "CREATE.h" +#include "DispCompIeqcJeqcIe.h" using namespace MbD; -void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEIpEJ() +MbD::OrbitAngleZIeqcJeqc::OrbitAngleZIeqcJeqc() +{ +} + +MbD::OrbitAngleZIeqcJeqc::OrbitAngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj) : OrbitAngleZIeqcJec(frmi, frmj) +{ +} + +void MbD::OrbitAngleZIeqcJeqc::calc_ppthezpEIpEJ() { auto pxpEI = xIeJeIe->pvaluepEI(); auto pypEI = yIeJeIe->pvaluepEI(); @@ -29,7 +39,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEIpEJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEIpXJ() +void MbD::OrbitAngleZIeqcJeqc::calc_ppthezpEIpXJ() { //ppxpEIpXJ = 0 //ppypEIpXJ = 0 @@ -54,7 +64,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEIpXJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEJpEJ() +void MbD::OrbitAngleZIeqcJeqc::calc_ppthezpEJpEJ() { auto pxpEJ = xIeJeIe->pvaluepEJ(); auto pypEJ = yIeJeIe->pvaluepEJ(); @@ -81,7 +91,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEJpEJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXIpEJ() +void MbD::OrbitAngleZIeqcJeqc::calc_ppthezpXIpEJ() { //ppxpXIpEJ = 0 //ppypXIpEJ = 0 @@ -106,7 +116,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXIpEJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXIpXJ() +void MbD::OrbitAngleZIeqcJeqc::calc_ppthezpXIpXJ() { //ppxpXIpXJ = 0 //ppypXIpXJ = 0 @@ -131,7 +141,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXIpXJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXJpEJ() +void MbD::OrbitAngleZIeqcJeqc::calc_ppthezpXJpEJ() { //ppxpXJpEJ = 0 //ppypXJpEJ = 0 @@ -156,7 +166,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXJpEJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXJpXJ() +void MbD::OrbitAngleZIeqcJeqc::calc_ppthezpXJpXJ() { //ppxpXJpXJ = 0 //ppypXJpXJ = 0 @@ -179,7 +189,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXJpXJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_pthezpEJ() +void MbD::OrbitAngleZIeqcJeqc::calc_pthezpEJ() { auto pxpEJ = xIeJeIe->pvaluepEJ(); auto pypEJ = yIeJeIe->pvaluepEJ(); @@ -189,7 +199,7 @@ void MbD::OrbitAnglezIeqcJeqc::calc_pthezpEJ() } } -void MbD::OrbitAnglezIeqcJeqc::calc_pthezpXJ() +void MbD::OrbitAngleZIeqcJeqc::calc_pthezpXJ() { auto pxpXJ = xIeJeIe->pvaluepXJ(); auto pypXJ = yIeJeIe->pvaluepXJ(); @@ -199,9 +209,9 @@ void MbD::OrbitAnglezIeqcJeqc::calc_pthezpXJ() } } -void MbD::OrbitAnglezIeqcJeqc::calcPostDynCorrectorIteration() +void MbD::OrbitAngleZIeqcJeqc::calcPostDynCorrectorIteration() { - OrbitAnglezIeqcJec::calcPostDynCorrectorIteration(); + OrbitAngleZIeqcJec::calcPostDynCorrectorIteration(); this->calc_pthezpXJ(); this->calc_pthezpEJ(); this->calc_ppthezpXIpXJ(); @@ -213,9 +223,15 @@ void MbD::OrbitAnglezIeqcJeqc::calcPostDynCorrectorIteration() this->calc_ppthezpEJpEJ(); } -void MbD::OrbitAnglezIeqcJeqc::initialize() +void MbD::OrbitAngleZIeqcJeqc::init_xyIeJeIe() { - OrbitAnglezIeqcJec::initialize(); + xIeJeIe = CREATE::With(frmI, frmJ, 0); + yIeJeIe = CREATE::With(frmI, frmJ, 1); +} + +void MbD::OrbitAngleZIeqcJeqc::initialize() +{ + OrbitAngleZIeqcJec::initialize(); pthezpXJ = std::make_shared>(3); pthezpEJ = std::make_shared>(4); ppthezpXIpXJ = std::make_shared>(3, 3); @@ -227,47 +243,47 @@ void MbD::OrbitAnglezIeqcJeqc::initialize() ppthezpEJpEJ = std::make_shared>(4, 4); } -FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepEIpEJ() +FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepEIpEJ() { return ppthezpEIpEJ; } -FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepEIpXJ() +FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepEIpXJ() { return ppthezpEIpXJ; } -FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepEJpEJ() +FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepEJpEJ() { return ppthezpEJpEJ; } -FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXIpEJ() +FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepXIpEJ() { return ppthezpXIpEJ; } -FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXIpXJ() +FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepXIpXJ() { return ppthezpXIpXJ; } -FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXJpEJ() +FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepXJpEJ() { return ppthezpXJpEJ; } -FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXJpXJ() +FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepXJpXJ() { return ppthezpXJpXJ; } -FRowDsptr MbD::OrbitAnglezIeqcJeqc::pvaluepEJ() +FRowDsptr MbD::OrbitAngleZIeqcJeqc::pvaluepEJ() { return pthezpEJ; } -FRowDsptr MbD::OrbitAnglezIeqcJeqc::pvaluepXJ() +FRowDsptr MbD::OrbitAngleZIeqcJeqc::pvaluepXJ() { return pthezpXJ; } diff --git a/MbDCode/OrbitAnglezIeqcJeqc.h b/MbDCode/OrbitAnglezIeqcJeqc.h index 2a4dcce..8efb42e 100644 --- a/MbDCode/OrbitAnglezIeqcJeqc.h +++ b/MbDCode/OrbitAnglezIeqcJeqc.h @@ -1,36 +1,40 @@ #pragma once -#include "OrbitAnglezIeqcJec.h" +#include "OrbitAngleZIeqcJec.h" namespace MbD { - class OrbitAnglezIeqcJeqc : public OrbitAnglezIeqcJec - { - //pthezpXJ pthezpEJ ppthezpXIpXJ ppthezpXIpEJ ppthezpEIpXJ ppthezpEIpEJ ppthezpXJpXJ ppthezpXJpEJ ppthezpEJpEJ - public: - void calc_ppthezpEIpEJ(); - void calc_ppthezpEIpXJ(); - void calc_ppthezpEJpEJ(); - void calc_ppthezpXIpEJ(); - void calc_ppthezpXIpXJ(); - void calc_ppthezpXJpEJ(); - void calc_ppthezpXJpXJ(); - void calc_pthezpEJ(); - void calc_pthezpXJ(); - void calcPostDynCorrectorIteration() override; - void initialize() override; - FMatDsptr ppvaluepEIpEJ() override; - FMatDsptr ppvaluepEIpXJ() override; - FMatDsptr ppvaluepEJpEJ() override; - FMatDsptr ppvaluepXIpEJ() override; - FMatDsptr ppvaluepXIpXJ() override; - FMatDsptr ppvaluepXJpEJ() override; - FMatDsptr ppvaluepXJpXJ() override; - FRowDsptr pvaluepEJ() override; - FRowDsptr pvaluepXJ() override; + class OrbitAngleZIeqcJeqc : public OrbitAngleZIeqcJec + { + //pthezpXJ pthezpEJ ppthezpXIpXJ ppthezpXIpEJ ppthezpEIpXJ ppthezpEIpEJ ppthezpXJpXJ ppthezpXJpEJ ppthezpEJpEJ + public: + OrbitAngleZIeqcJeqc(); + OrbitAngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calc_ppthezpEIpEJ(); + void calc_ppthezpEIpXJ(); + void calc_ppthezpEJpEJ(); + void calc_ppthezpXIpEJ(); + void calc_ppthezpXIpXJ(); + void calc_ppthezpXJpEJ(); + void calc_ppthezpXJpXJ(); + void calc_pthezpEJ(); + void calc_pthezpXJ(); + void calcPostDynCorrectorIteration() override; + void init_xyIeJeIe() override; + void initialize() override; + FMatDsptr ppvaluepEIpEJ() override; + FMatDsptr ppvaluepEIpXJ() override; + FMatDsptr ppvaluepEJpEJ() override; + FMatDsptr ppvaluepXIpEJ() override; + FMatDsptr ppvaluepXIpXJ() override; + FMatDsptr ppvaluepXJpEJ() override; + FMatDsptr ppvaluepXJpXJ() override; + FRowDsptr pvaluepEJ() override; + FRowDsptr pvaluepXJ() override; - FRowDsptr pthezpXJ, pthezpEJ; - FMatDsptr ppthezpXIpXJ, ppthezpXIpEJ, ppthezpEIpXJ, ppthezpEIpEJ, ppthezpXJpXJ, ppthezpXJpEJ, ppthezpEJpEJ; - }; + FRowDsptr pthezpXJ, pthezpEJ; + FMatDsptr ppthezpXIpXJ, ppthezpXIpEJ, ppthezpEIpXJ, ppthezpEIpEJ, ppthezpXJpXJ, ppthezpXJpEJ, ppthezpEJpEJ; + }; } diff --git a/MbDCode/Orientation.cpp b/MbDCode/Orientation.cpp new file mode 100644 index 0000000..7706e2d --- /dev/null +++ b/MbDCode/Orientation.cpp @@ -0,0 +1,31 @@ +#include "Orientation.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::Orientation::Orientation() +{ +} + +MbD::Orientation::Orientation(const char* str) +{ +} + +void MbD::Orientation::initializeGlobally() +{ + if (constraints->empty()) { + initMotions(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + PrescribedMotion::initializeGlobally(); + } +} + +void MbD::Orientation::initMotions() +{ +} diff --git a/MbDCode/Orientation.h b/MbDCode/Orientation.h new file mode 100644 index 0000000..e916554 --- /dev/null +++ b/MbDCode/Orientation.h @@ -0,0 +1,17 @@ +#pragma once + +#include "PrescribedMotion.h" + +namespace MbD { + class Orientation : public PrescribedMotion + { + // + public: + Orientation(); + Orientation(const char* str); + void initializeGlobally() override; + void initMotions() override; + + }; +} + diff --git a/MbDCode/ParallelAxesJoint.cpp b/MbDCode/ParallelAxesJoint.cpp new file mode 100644 index 0000000..152fc2e --- /dev/null +++ b/MbDCode/ParallelAxesJoint.cpp @@ -0,0 +1,26 @@ +#include "ParallelAxesJoint.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::ParallelAxesJoint::ParallelAxesJoint() +{ +} + +MbD::ParallelAxesJoint::ParallelAxesJoint(const char* str) +{ +} + +void MbD::ParallelAxesJoint::initializeGlobally() +{ + if (constraints->empty()) + { + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/ParallelAxesJoint.h b/MbDCode/ParallelAxesJoint.h new file mode 100644 index 0000000..640239e --- /dev/null +++ b/MbDCode/ParallelAxesJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class ParallelAxesJoint : public Joint + { + // + public: + ParallelAxesJoint(); + ParallelAxesJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 2f98d7e..68ff87e 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -1,4 +1,5 @@ #pragma once + #include #include "Item.h" diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h index 551222e..9b622cc 100644 --- a/MbDCode/PartFrame.h +++ b/MbDCode/PartFrame.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/PerpendicularJoint.cpp b/MbDCode/PerpendicularJoint.cpp new file mode 100644 index 0000000..55b9b22 --- /dev/null +++ b/MbDCode/PerpendicularJoint.cpp @@ -0,0 +1,25 @@ +#include "PerpendicularJoint.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::PerpendicularJoint::PerpendicularJoint() +{ +} + +MbD::PerpendicularJoint::PerpendicularJoint(const char* str) +{ +} + +void MbD::PerpendicularJoint::initializeGlobally() +{ + if (constraints->empty()) + { + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 2)); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/PerpendicularJoint.h b/MbDCode/PerpendicularJoint.h new file mode 100644 index 0000000..5f0f5f5 --- /dev/null +++ b/MbDCode/PerpendicularJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class PerpendicularJoint : public Joint + { + // + public: + PerpendicularJoint(); + PerpendicularJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/PlanarJoint.cpp b/MbDCode/PlanarJoint.cpp new file mode 100644 index 0000000..0592184 --- /dev/null +++ b/MbDCode/PlanarJoint.cpp @@ -0,0 +1,27 @@ +#include "PlanarJoint.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::PlanarJoint::PlanarJoint() +{ +} + +MbD::PlanarJoint::PlanarJoint(const char* str) +{ +} + +void MbD::PlanarJoint::initializeGlobally() +{ + if (constraints->empty()) + { + this->createInPlaneConstraint(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + InPlaneJoint::initializeGlobally(); + } +} diff --git a/MbDCode/PlanarJoint.h b/MbDCode/PlanarJoint.h new file mode 100644 index 0000000..a6ee9ed --- /dev/null +++ b/MbDCode/PlanarJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "InPlaneJoint.h" + +namespace MbD { + class PlanarJoint : public InPlaneJoint + { + // + public: + PlanarJoint(); + PlanarJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/PointInLineJoint.cpp b/MbDCode/PointInLineJoint.cpp new file mode 100644 index 0000000..c27d185 --- /dev/null +++ b/MbDCode/PointInLineJoint.cpp @@ -0,0 +1,25 @@ +#include "PointInLineJoint.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::PointInLineJoint::PointInLineJoint() +{ +} + +MbD::PointInLineJoint::PointInLineJoint(const char* str) : InLineJoint(str) +{ +} + +void MbD::PointInLineJoint::initializeGlobally() +{ + if (constraints->empty()) + { + createInLineConstraints(); + this->root()->hasChanged = true; + } + else { + InLineJoint::initializeGlobally(); + } +} diff --git a/MbDCode/PointInLineJoint.h b/MbDCode/PointInLineJoint.h new file mode 100644 index 0000000..42ff2d1 --- /dev/null +++ b/MbDCode/PointInLineJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "InLineJoint.h" + +namespace MbD { + class PointInLineJoint : public InLineJoint + { + // + public: + PointInLineJoint(); + PointInLineJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/PointInPlaneJoint.cpp b/MbDCode/PointInPlaneJoint.cpp new file mode 100644 index 0000000..a474f8f --- /dev/null +++ b/MbDCode/PointInPlaneJoint.cpp @@ -0,0 +1,24 @@ +#include "PointInPlaneJoint.h" +#include "System.h" + +using namespace MbD; + +MbD::PointInPlaneJoint::PointInPlaneJoint() +{ +} + +MbD::PointInPlaneJoint::PointInPlaneJoint(const char* str) +{ +} + +void MbD::PointInPlaneJoint::initializeGlobally() +{ + if (constraints->empty()) + { + this->createInPlaneConstraint(); + this->root()->hasChanged = true; + } + else { + InPlaneJoint::initializeGlobally(); + } +} diff --git a/MbDCode/PointInPlaneJoint.h b/MbDCode/PointInPlaneJoint.h new file mode 100644 index 0000000..0cf4f36 --- /dev/null +++ b/MbDCode/PointInPlaneJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "InPlaneJoint.h" + +namespace MbD { + class PointInPlaneJoint : public InPlaneJoint + { + // + public: + PointInPlaneJoint(); + PointInPlaneJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/PrescribedMotion.cpp b/MbDCode/PrescribedMotion.cpp index 527c4d3..682ebc1 100644 --- a/MbDCode/PrescribedMotion.cpp +++ b/MbDCode/PrescribedMotion.cpp @@ -28,6 +28,11 @@ void PrescribedMotion::initialize() psiBlk = std::make_shared(0.0); } +void MbD::PrescribedMotion::initMotions() +{ + assert(false); +} + void PrescribedMotion::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj) { Joint::connectsItoJ(frmi, frmj); diff --git a/MbDCode/PrescribedMotion.h b/MbDCode/PrescribedMotion.h index 5505925..eefe604 100644 --- a/MbDCode/PrescribedMotion.h +++ b/MbDCode/PrescribedMotion.h @@ -1,4 +1,5 @@ #pragma once + #include "Joint.h" #include "Symbolic.h" @@ -10,8 +11,10 @@ namespace MbD { public: PrescribedMotion(); PrescribedMotion(const char* str); - void initialize() override; + void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override; + void initialize() override; + virtual void initMotions(); Symsptr xBlk; Symsptr yBlk; diff --git a/MbDCode/QuasiIntegrator.h b/MbDCode/QuasiIntegrator.h index 835651b..d619410 100644 --- a/MbDCode/QuasiIntegrator.h +++ b/MbDCode/QuasiIntegrator.h @@ -1,4 +1,5 @@ #pragma once + #include #include "IntegratorInterface.h" diff --git a/MbDCode/RackPinConstraintIJ.cpp b/MbDCode/RackPinConstraintIJ.cpp new file mode 100644 index 0000000..d9ee8e4 --- /dev/null +++ b/MbDCode/RackPinConstraintIJ.cpp @@ -0,0 +1,80 @@ +#include "RackPinConstraintIJ.h" + +using namespace MbD; + +MbD::RackPinConstraintIJ::RackPinConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : ConstraintIJ(frmi, frmj) +{ +} + +void MbD::RackPinConstraintIJ::calcPostDynCorrectorIteration() +{ + auto x = xIeJeIe->value(); + auto thez = thezIeJe->value(); + aG = x + (pitchRadius * thez) - aConstant; +} + +void MbD::RackPinConstraintIJ::init_xthez() +{ + assert(false); +} + +void MbD::RackPinConstraintIJ::initialize() +{ + ConstraintIJ::initialize(); + this->init_xthez(); +} + +void MbD::RackPinConstraintIJ::initializeGlobally() +{ + xIeJeIe->initializeGlobally(); + thezIeJe->initializeGlobally(); +} + +void MbD::RackPinConstraintIJ::initializeLocally() +{ + xIeJeIe->initializeLocally(); + thezIeJe->initializeLocally(); +} + +void MbD::RackPinConstraintIJ::postInput() +{ + xIeJeIe->postInput(); + thezIeJe->postInput(); + aConstant = xIeJeIe->value() + (pitchRadius * thezIeJe->value()); + ConstraintIJ::postInput(); +} + +void MbD::RackPinConstraintIJ::postPosICIteration() +{ + xIeJeIe->postPosICIteration(); + thezIeJe->postPosICIteration(); + ConstraintIJ::postPosICIteration(); +} + +void MbD::RackPinConstraintIJ::preAccIC() +{ + xIeJeIe->preAccIC(); + thezIeJe->preAccIC(); + ConstraintIJ::preAccIC(); +} + +void MbD::RackPinConstraintIJ::prePosIC() +{ + xIeJeIe->prePosIC(); + thezIeJe->prePosIC(); + ConstraintIJ::prePosIC(); +} + +void MbD::RackPinConstraintIJ::preVelIC() +{ + xIeJeIe->preVelIC(); + thezIeJe->preVelIC(); + ConstraintIJ::preVelIC(); +} + +void MbD::RackPinConstraintIJ::simUpdateAll() +{ + xIeJeIe->simUpdateAll(); + thezIeJe->simUpdateAll(); + ConstraintIJ::simUpdateAll(); +} diff --git a/MbDCode/RackPinConstraintIJ.h b/MbDCode/RackPinConstraintIJ.h new file mode 100644 index 0000000..777593c --- /dev/null +++ b/MbDCode/RackPinConstraintIJ.h @@ -0,0 +1,31 @@ +#pragma once + +#include "ConstraintIJ.h" +#include "DispCompIecJecIe.h" +#include "AngleZIecJec.h" + +namespace MbD { + class RackPinConstraintIJ : public ConstraintIJ + { + //xIeJeIe thezIeJe pitchRadius + public: + RackPinConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + virtual void init_xthez(); + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + + std::shared_ptr xIeJeIe; + std::shared_ptr thezIeJe; + double pitchRadius; + }; +} + diff --git a/MbDCode/RackPinConstraintIqcJc.cpp b/MbDCode/RackPinConstraintIqcJc.cpp new file mode 100644 index 0000000..21fb7c0 --- /dev/null +++ b/MbDCode/RackPinConstraintIqcJc.cpp @@ -0,0 +1,123 @@ +#include "RackPinConstraintIqcJc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "AngleZIeqcJec.h" +#include "DispCompIeqcJecIe.h" + +using namespace MbD; + +MbD::RackPinConstraintIqcJc::RackPinConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : RackPinConstraintIJ(frmi, frmj) +{ +} + +void MbD::RackPinConstraintIqcJc::addToJointForceI(FColDsptr col) +{ + col->equalSelfPlusFullVectortimes(pGpXI, lam); +} + +void MbD::RackPinConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto cForceT = pGpXI->times(lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto rIpIeIp = frmIeqc->rpep(); + auto pAOIppEI = frmIeqc->pAOppE(); + auto aBOIp = frmIeqc->aBOp(); + auto fpAOIppEIrIpIeIp = std::make_shared>(4, 0.0); + for (int i = 0; i < 4; i++) + { + auto dum = cForceT->timesFullColumn(pAOIppEI->at(i)->timesFullColumn(rIpIeIp)); + fpAOIppEIrIpIeIp->atiput(i, dum); + } + auto lampGpE = pGpEI->transpose()->times(lam); + auto c2Torque = aBOIp->timesFullColumn(lampGpE->minusFullColumn(fpAOIppEIrIpIeIp)); + jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); +} + +void MbD::RackPinConstraintIqcJc::calc_pGpEI() +{ + pGpEI = xIeJeIe->pvaluepEI()->plusFullRow(thezIeJe->pvaluepEI()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJc::calc_pGpXI() +{ + pGpXI = xIeJeIe->pvaluepXI()->plusFullRow(thezIeJe->pvaluepXI()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJc::calc_ppGpEIpEI() +{ + ppGpEIpEI = xIeJeIe->ppvaluepEIpEI()->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJc::calc_ppGpXIpEI() +{ + ppGpXIpEI = xIeJeIe->ppvaluepXIpEI()->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJc::calcPostDynCorrectorIteration() +{ + RackPinConstraintIJ::calcPostDynCorrectorIteration(); + this->calc_pGpXI(); + this->calc_pGpEI(); + this->calc_ppGpXIpEI(); + this->calc_ppGpEIpEI(); +} + +void MbD::RackPinConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpXI->timesFullColumn(efrmIqc->qXddot()); + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEI->timesFullColumn(qEdotI))); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} + +void MbD::RackPinConstraintIqcJc::fillPosICError(FColDsptr col) +{ + RackPinConstraintIJ::fillPosICError(col); + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::RackPinConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::RackPinConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); +} + +void MbD::RackPinConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::RackPinConstraintIqcJc::init_xthez() +{ + xIeJeIe = CREATE::With(frmI, frmJ, 0); + thezIeJe = CREATE::With(frmJ, frmI); +} + +void MbD::RackPinConstraintIqcJc::useEquationNumbers() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + iqXI = frmIeqc->iqX(); + iqEI = frmIeqc->iqE(); +} diff --git a/MbDCode/RackPinConstraintIqcJc.h b/MbDCode/RackPinConstraintIqcJc.h new file mode 100644 index 0000000..df200da --- /dev/null +++ b/MbDCode/RackPinConstraintIqcJc.h @@ -0,0 +1,33 @@ +#pragma once + +#include "RackPinConstraintIJ.h" + +namespace MbD { + class RackPinConstraintIqcJc : public RackPinConstraintIJ + { + //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI + public: + RackPinConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj); + + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; + void calc_pGpEI(); + void calc_pGpXI(); + void calc_ppGpEIpEI(); + void calc_ppGpXIpEI(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void init_xthez() override; + void useEquationNumbers() override; + + FRowDsptr pGpXI, pGpEI; + FMatDsptr ppGpXIpEI, ppGpEIpEI; + int iqXI, iqEI; + + }; +} + diff --git a/MbDCode/RackPinConstraintIqcJqc.cpp b/MbDCode/RackPinConstraintIqcJqc.cpp new file mode 100644 index 0000000..3328aef --- /dev/null +++ b/MbDCode/RackPinConstraintIqcJqc.cpp @@ -0,0 +1,118 @@ +#include "RackPinConstraintIqcJqc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "DispCompIeqcJeqcIe.h" +#include "AngleZIeqcJeqc.h" + +using namespace MbD; + +MbD::RackPinConstraintIqcJqc::RackPinConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj) : RackPinConstraintIqcJc(frmi, frmj) +{ +} + +void MbD::RackPinConstraintIqcJqc::calc_pGpEJ() +{ + pGpEJ = xIeJeIe->pvaluepEJ()->plusFullRow(thezIeJe->pvaluepEJ()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJqc::calc_pGpXJ() +{ + pGpXJ = xIeJeIe->pvaluepXJ()->plusFullRow(thezIeJe->pvaluepXJ()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpEJ() +{ + ppGpEIpEJ = xIeJeIe->ppvaluepEIpEJ()->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpXJ() +{ + ppGpEIpXJ = xIeJeIe->ppvaluepEIpXJ()->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJqc::calc_ppGpEJpEJ() +{ + ppGpEJpEJ = xIeJeIe->ppvaluepEJpEJ()->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius)); +} + +void MbD::RackPinConstraintIqcJqc::calcPostDynCorrectorIteration() +{ + RackPinConstraintIqcJc::calcPostDynCorrectorIteration(); + this->calc_pGpXJ(); + this->calc_pGpEJ(); + this->calc_ppGpEIpXJ(); + this->calc_ppGpEIpEJ(); + this->calc_ppGpEJpEJ(); +} + +void MbD::RackPinConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + RackPinConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto frmJeqc = std::static_pointer_cast(frmJ); + auto qEdotI = frmIeqc->qEdot(); + auto qXdotJ = frmJeqc->qXdot(); + auto qEdotJ = frmJeqc->qEdot(); + double sum = 0.0; + sum += pGpXJ->timesFullColumn(frmJeqc->qXddot()); + sum += pGpEJ->timesFullColumn(frmJeqc->qEddot()); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpXJ->timesFullColumn(qXdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} + +void MbD::RackPinConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + RackPinConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::RackPinConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + RackPinConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::RackPinConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + RackPinConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} + +void MbD::RackPinConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + RackPinConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::RackPinConstraintIqcJqc::init_xthez() +{ + xIeJeIe = CREATE::With(frmI, frmJ, 0); + thezIeJe = CREATE::With(frmJ, frmI); +} + +void MbD::RackPinConstraintIqcJqc::useEquationNumbers() +{ + RackPinConstraintIqcJc::useEquationNumbers(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqXJ = frmJeqc->iqX(); + iqEJ = frmJeqc->iqE(); +} diff --git a/MbDCode/RackPinConstraintIqcJqc.h b/MbDCode/RackPinConstraintIqcJqc.h new file mode 100644 index 0000000..fdd6bd7 --- /dev/null +++ b/MbDCode/RackPinConstraintIqcJqc.h @@ -0,0 +1,31 @@ +#pragma once + +#include "RackPinConstraintIqcJc.h" + +namespace MbD { + class RackPinConstraintIqcJqc : public RackPinConstraintIqcJc + { + //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ + public: + RackPinConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calc_pGpEJ(); + void calc_pGpXJ(); + void calc_ppGpEIpEJ(); + void calc_ppGpEIpXJ(); + void calc_ppGpEJpEJ(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void init_xthez() override; + void useEquationNumbers() override; + + FRowDsptr pGpXJ, pGpEJ; + FMatDsptr ppGpEIpXJ, ppGpEIpEJ, ppGpEJpEJ; + int iqXJ = -1, iqEJ = -1; + }; +} + diff --git a/MbDCode/RackPinJoint.cpp b/MbDCode/RackPinJoint.cpp new file mode 100644 index 0000000..b8d2f70 --- /dev/null +++ b/MbDCode/RackPinJoint.cpp @@ -0,0 +1,40 @@ +#include "RackPinJoint.h" +#include "CREATE.h" +#include "System.h" +#include "RackPinConstraintIJ.h" + +using namespace MbD; + +MbD::RackPinJoint::RackPinJoint() +{ +} + +MbD::RackPinJoint::RackPinJoint(const char* str) : Joint(str) +{ +} + +void MbD::RackPinJoint::initializeGlobally() +{ + if (constraints->empty()) + { + auto rackPinIJ = CREATE::With(frmI, frmJ); + rackPinIJ->setConstant(aConstant); + rackPinIJ->pitchRadius = pitchRadius; + addConstraint(rackPinIJ); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} + +void MbD::RackPinJoint::connectsItoJ(EndFrmcptr frmIe, EndFrmcptr frmJe) +{ + //"OODS J is on pinion. z axis is axis of pinion." + //"OODS I is on rack. x axis is axis of rack. z axis is parallel to axis of pinion." + //"Subsequent prescribed motions may make frmIe, frmJe become prescribed end frames." + //"Use newCopyEndFrameqc to prevent efrms from becoming EndFrameqct." + + frmI = frmIe->newCopyEndFrameqc(); + frmJ = frmJe->newCopyEndFrameqc(); +} diff --git a/MbDCode/RackPinJoint.h b/MbDCode/RackPinJoint.h new file mode 100644 index 0000000..4f8ee25 --- /dev/null +++ b/MbDCode/RackPinJoint.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class RackPinJoint : public Joint + { + //pitchRadius aConstant + public: + RackPinJoint(); + RackPinJoint(const char* str); + void initializeGlobally() override; + void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override; + + double pitchRadius, aConstant; + }; +} + diff --git a/MbDCode/RevCylJoint.cpp b/MbDCode/RevCylJoint.cpp new file mode 100644 index 0000000..b268672 --- /dev/null +++ b/MbDCode/RevCylJoint.cpp @@ -0,0 +1,30 @@ +#include "RevCylJoint.h" +#include "CREATE.h" +#include "DistancexyConstraintIJ.h" +#include "System.h" + +using namespace MbD; + +MbD::RevCylJoint::RevCylJoint() +{ +} + +MbD::RevCylJoint::RevCylJoint(const char* str) : CompoundJoint(str) +{ +} + +void MbD::RevCylJoint::initializeGlobally() +{ + if (constraints->empty()) + { + auto distxyIJ = CREATE::With(frmI, frmJ); + distxyIJ->setConstant(distanceIJ); + addConstraint(distxyIJ); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + CompoundJoint::initializeGlobally(); + } +} diff --git a/MbDCode/RevCylJoint.h b/MbDCode/RevCylJoint.h new file mode 100644 index 0000000..c32109e --- /dev/null +++ b/MbDCode/RevCylJoint.h @@ -0,0 +1,17 @@ +#pragma once + +#include "CompoundJoint.h" + +namespace MbD { + class RevCylJoint : public CompoundJoint + { + // + public: + RevCylJoint(); + RevCylJoint(const char* str); + void initializeGlobally() override; + + + }; +} + diff --git a/MbDCode/RevoluteJoint.cpp b/MbDCode/RevoluteJoint.cpp index 97ec2d6..80509f7 100644 --- a/MbDCode/RevoluteJoint.cpp +++ b/MbDCode/RevoluteJoint.cpp @@ -6,21 +6,19 @@ using namespace MbD; -RevoluteJoint::RevoluteJoint() { - +RevoluteJoint::RevoluteJoint() +{ } -RevoluteJoint::RevoluteJoint(const char* str) : Joint(str) { - +RevoluteJoint::RevoluteJoint(const char* str) : AtPointJoint(str) +{ } void RevoluteJoint::initializeGlobally() { if (constraints->empty()) { - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); - addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2)); + createAtPointConstraints(); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); this->root()->hasChanged = true; diff --git a/MbDCode/RevoluteJoint.h b/MbDCode/RevoluteJoint.h index fe9c71e..5c6ddb8 100644 --- a/MbDCode/RevoluteJoint.h +++ b/MbDCode/RevoluteJoint.h @@ -1,7 +1,9 @@ #pragma once -#include "Joint.h" + +#include "AtPointJoint.h" + namespace MbD { - class RevoluteJoint : public Joint + class RevoluteJoint : public AtPointJoint { // public: diff --git a/MbDCode/ScrewConstraintIJ.cpp b/MbDCode/ScrewConstraintIJ.cpp new file mode 100644 index 0000000..1e57e94 --- /dev/null +++ b/MbDCode/ScrewConstraintIJ.cpp @@ -0,0 +1,82 @@ +#include + +#include "ScrewConstraintIJ.h" + +using namespace MbD; + +MbD::ScrewConstraintIJ::ScrewConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : ConstraintIJ(frmi, frmj) +{ +} + +void MbD::ScrewConstraintIJ::calcPostDynCorrectorIteration() +{ + auto z = zIeJeIe->value(); + auto thez = thezIeJe->value(); + aG = (2.0 * M_PI * z) - (pitch * thez) - aConstant; +} + +void MbD::ScrewConstraintIJ::init_zthez() +{ + assert(false); +} + +void MbD::ScrewConstraintIJ::initialize() +{ + ConstraintIJ::initialize(); + this->init_zthez(); +} + +void MbD::ScrewConstraintIJ::initializeGlobally() +{ + zIeJeIe->initializeGlobally(); + thezIeJe->initializeGlobally(); +} + +void MbD::ScrewConstraintIJ::initializeLocally() +{ + zIeJeIe->initializeLocally(); + thezIeJe->initializeLocally(); +} + +void MbD::ScrewConstraintIJ::postInput() +{ + zIeJeIe->postInput(); + thezIeJe->postInput(); + aConstant = (2.0 * M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch); + ConstraintIJ::postInput(); +} + +void MbD::ScrewConstraintIJ::postPosICIteration() +{ + zIeJeIe->postPosICIteration(); + thezIeJe->postPosICIteration(); + ConstraintIJ::postPosICIteration(); +} + +void MbD::ScrewConstraintIJ::preAccIC() +{ + zIeJeIe->preAccIC(); + thezIeJe->preAccIC(); + ConstraintIJ::preAccIC(); +} + +void MbD::ScrewConstraintIJ::prePosIC() +{ + zIeJeIe->prePosIC(); + thezIeJe->prePosIC(); + ConstraintIJ::prePosIC(); +} + +void MbD::ScrewConstraintIJ::preVelIC() +{ + zIeJeIe->preVelIC(); + thezIeJe->preVelIC(); + ConstraintIJ::preVelIC(); +} + +void MbD::ScrewConstraintIJ::simUpdateAll() +{ + zIeJeIe->simUpdateAll(); + thezIeJe->simUpdateAll(); + ConstraintIJ::simUpdateAll(); +} diff --git a/MbDCode/ScrewConstraintIJ.h b/MbDCode/ScrewConstraintIJ.h new file mode 100644 index 0000000..dfe33a6 --- /dev/null +++ b/MbDCode/ScrewConstraintIJ.h @@ -0,0 +1,33 @@ +#pragma once + +#include "ConstraintIJ.h" +#include "DispCompIecJecIe.h" +#include "AngleZIecJec.h" + +namespace MbD { + class ScrewConstraintIJ : public ConstraintIJ + { + //zIeJeIe thezIeJe pitch + public: + ScrewConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + virtual void init_zthez(); + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + + std::shared_ptr zIeJeIe; + std::shared_ptr thezIeJe; + double pitch; + + + }; +} + diff --git a/MbDCode/ScrewConstraintIqcJc.cpp b/MbDCode/ScrewConstraintIqcJc.cpp new file mode 100644 index 0000000..be49b11 --- /dev/null +++ b/MbDCode/ScrewConstraintIqcJc.cpp @@ -0,0 +1,125 @@ +#include + +#include "ScrewConstraintIqcJc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "DispCompIeqcJecIe.h" +#include "AngleZIeqcJec.h" + +using namespace MbD; + +MbD::ScrewConstraintIqcJc::ScrewConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : ScrewConstraintIJ(frmi, frmj) +{ +} + +void MbD::ScrewConstraintIqcJc::addToJointForceI(FColDsptr col) +{ + col->equalSelfPlusFullVectortimes(pGpXI, lam); +} + +void MbD::ScrewConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto cForceT = pGpXI->times(lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto rIpIeIp = frmIeqc->rpep(); + auto pAOIppEI = frmIeqc->pAOppE(); + auto aBOIp = frmIeqc->aBOp(); + auto fpAOIppEIrIpIeIp = std::make_shared>(4, 0.0); + for (int i = 0; i < 4; i++) + { + auto dum = cForceT->timesFullColumn(pAOIppEI->at(i)->timesFullColumn(rIpIeIp)); + fpAOIppEIrIpIeIp->atiput(i, dum); + } + auto lampGpE = pGpEI->transpose()->times(lam); + auto c2Torque = aBOIp->timesFullColumn(lampGpE->minusFullColumn(fpAOIppEIrIpIeIp)); + jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); +} + +void MbD::ScrewConstraintIqcJc::calc_pGpEI() +{ + pGpEI = zIeJeIe->pvaluepEI()->times(2.0 * M_PI)->minusFullRow(thezIeJe->pvaluepEI()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJc::calc_pGpXI() +{ + pGpXI = zIeJeIe->pvaluepXI()->times(2.0 * M_PI)->minusFullRow(thezIeJe->pvaluepXI()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJc::calc_ppGpEIpEI() +{ + ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJc::calc_ppGpXIpEI() +{ + ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJc::calcPostDynCorrectorIteration() +{ + ScrewConstraintIJ::calcPostDynCorrectorIteration(); + this->calc_pGpXI(); + this->calc_pGpEI(); + this->calc_ppGpXIpEI(); + this->calc_ppGpEIpEI(); +} + +void MbD::ScrewConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpXI->timesFullColumn(efrmIqc->qXddot()); + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEI->timesFullColumn(qEdotI))); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} + +void MbD::ScrewConstraintIqcJc::fillPosICError(FColDsptr col) +{ + ScrewConstraintIJ::fillPosICError(col); + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::ScrewConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::ScrewConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); +} + +void MbD::ScrewConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::ScrewConstraintIqcJc::init_zthez() +{ + zIeJeIe = CREATE::With(frmI, frmJ, 2); + thezIeJe = CREATE::With(frmJ, frmI); +} + +void MbD::ScrewConstraintIqcJc::useEquationNumbers() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + iqXI = frmIeqc->iqX(); + iqEI = frmIeqc->iqE(); +} diff --git a/MbDCode/ScrewConstraintIqcJc.h b/MbDCode/ScrewConstraintIqcJc.h new file mode 100644 index 0000000..241ea3b --- /dev/null +++ b/MbDCode/ScrewConstraintIqcJc.h @@ -0,0 +1,34 @@ +#pragma once + +#include "ScrewConstraintIJ.h" + +namespace MbD { + class ScrewConstraintIqcJc : public ScrewConstraintIJ + { + //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI + public: + ScrewConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj); + + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; + void calc_pGpEI(); + void calc_pGpXI(); + void calc_ppGpEIpEI(); + void calc_ppGpXIpEI(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void init_zthez() override; + void useEquationNumbers() override; + + FRowDsptr pGpXI, pGpEI; + FMatDsptr ppGpXIpEI, ppGpEIpEI; + int iqXI, iqEI; + + + }; +} + diff --git a/MbDCode/ScrewConstraintIqcJqc.cpp b/MbDCode/ScrewConstraintIqcJqc.cpp new file mode 100644 index 0000000..c76bc10 --- /dev/null +++ b/MbDCode/ScrewConstraintIqcJqc.cpp @@ -0,0 +1,120 @@ +#include + +#include "ScrewConstraintIqcJqc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "DispCompIeqcJeqcIe.h" +#include "AngleZIeqcJeqc.h" + +using namespace MbD; + +MbD::ScrewConstraintIqcJqc::ScrewConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj) : ScrewConstraintIqcJc(frmi, frmj) +{ +} + +void MbD::ScrewConstraintIqcJqc::calc_pGpEJ() +{ + pGpEJ = zIeJeIe->pvaluepEJ()->times(2.0 * M_PI)->minusFullRow(thezIeJe->pvaluepEJ()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJqc::calc_pGpXJ() +{ + pGpXJ = zIeJeIe->pvaluepXJ()->times(2.0 * M_PI)->minusFullRow(thezIeJe->pvaluepXJ()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpEJ() +{ + ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpXJ() +{ + ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJqc::calc_ppGpEJpEJ() +{ + ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch)); +} + +void MbD::ScrewConstraintIqcJqc::calcPostDynCorrectorIteration() +{ + ScrewConstraintIqcJc::calcPostDynCorrectorIteration(); + this->calc_pGpXJ(); + this->calc_pGpEJ(); + this->calc_ppGpEIpXJ(); + this->calc_ppGpEIpEJ(); + this->calc_ppGpEJpEJ(); +} + +void MbD::ScrewConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + ScrewConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto frmJeqc = std::static_pointer_cast(frmJ); + auto qEdotI = frmIeqc->qEdot(); + auto qXdotJ = frmJeqc->qXdot(); + auto qEdotJ = frmJeqc->qEdot(); + double sum = 0.0; + sum += pGpXJ->timesFullColumn(frmJeqc->qXddot()); + sum += pGpEJ->timesFullColumn(frmJeqc->qEddot()); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpXJ->timesFullColumn(qXdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} + +void MbD::ScrewConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + ScrewConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::ScrewConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + ScrewConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::ScrewConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + ScrewConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} + +void MbD::ScrewConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + ScrewConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::ScrewConstraintIqcJqc::init_zthez() +{ + zIeJeIe = CREATE::With(frmI, frmJ, 2); + thezIeJe = CREATE::With(frmJ, frmI); +} + +void MbD::ScrewConstraintIqcJqc::useEquationNumbers() +{ + ScrewConstraintIqcJc::useEquationNumbers(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqXJ = frmJeqc->iqX(); + iqEJ = frmJeqc->iqE(); +} diff --git a/MbDCode/ScrewConstraintIqcJqc.h b/MbDCode/ScrewConstraintIqcJqc.h new file mode 100644 index 0000000..3b877fd --- /dev/null +++ b/MbDCode/ScrewConstraintIqcJqc.h @@ -0,0 +1,33 @@ +#pragma once + +#include "ScrewConstraintIqcJc.h" + +namespace MbD { + class ScrewConstraintIqcJqc : public ScrewConstraintIqcJc + { + //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ + public: + ScrewConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calc_pGpEJ(); + void calc_pGpXJ(); + void calc_ppGpEIpEJ(); + void calc_ppGpEIpXJ(); + void calc_ppGpEJpEJ(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void init_zthez() override; + void useEquationNumbers() override; + + FRowDsptr pGpXJ, pGpEJ; + FMatDsptr ppGpEIpXJ, ppGpEIpEJ, ppGpEJpEJ; + int iqXJ = -1, iqEJ = -1; + + + }; +} + diff --git a/MbDCode/ScrewJoint.cpp b/MbDCode/ScrewJoint.cpp new file mode 100644 index 0000000..eecbf3d --- /dev/null +++ b/MbDCode/ScrewJoint.cpp @@ -0,0 +1,38 @@ +#include "ScrewJoint.h" +#include "CREATE.h" +#include "System.h" +#include "ScrewConstraintIJ.h" + +using namespace MbD; + +MbD::ScrewJoint::ScrewJoint() +{ +} + +MbD::ScrewJoint::ScrewJoint(const char* str) : Joint(str) +{ +} + +void MbD::ScrewJoint::initializeGlobally() +{ + if (constraints->empty()) + { + auto screwIJ = CREATE::With(frmI, frmJ); + screwIJ->setConstant(aConstant); + screwIJ->pitch = pitch; + addConstraint(screwIJ); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} + +void MbD::ScrewJoint::connectsItoJ(EndFrmcptr frmIe, EndFrmcptr frmJe) +{ + //"Subsequent prescribed motions may make frmIe, frmJe become prescribed end frames." + //"Use newCopyEndFrameqc to prevent efrms from becoming EndFrameqct." + + frmI = frmIe->newCopyEndFrameqc(); + frmJ = frmJe->newCopyEndFrameqc(); +} diff --git a/MbDCode/ScrewJoint.h b/MbDCode/ScrewJoint.h new file mode 100644 index 0000000..5971cd4 --- /dev/null +++ b/MbDCode/ScrewJoint.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class ScrewJoint : public Joint + { + // + public: + ScrewJoint(); + ScrewJoint(const char* str); + void initializeGlobally() override; + void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override; + + double pitch, aConstant; + }; +} + diff --git a/MbDCode/SimulationStoppingError.h b/MbDCode/SimulationStoppingError.h index ce43ed4..0fcf478 100644 --- a/MbDCode/SimulationStoppingError.h +++ b/MbDCode/SimulationStoppingError.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/SingularMatrixError.h b/MbDCode/SingularMatrixError.h index 276f25e..1488e13 100644 --- a/MbDCode/SingularMatrixError.h +++ b/MbDCode/SingularMatrixError.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/SparseColumn.h b/MbDCode/SparseColumn.h index 38709c3..fb32fcd 100644 --- a/MbDCode/SparseColumn.h +++ b/MbDCode/SparseColumn.h @@ -1,4 +1,5 @@ #pragma once + #include "SparseVector.h" namespace MbD { diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h index 77ecd7d..7402750 100644 --- a/MbDCode/SparseMatrix.h +++ b/MbDCode/SparseMatrix.h @@ -1,4 +1,5 @@ #pragma once + #include #include "RowTypeMatrix.h" diff --git a/MbDCode/SparseRow.h b/MbDCode/SparseRow.h index 1a78353..42d63a0 100644 --- a/MbDCode/SparseRow.h +++ b/MbDCode/SparseRow.h @@ -1,4 +1,5 @@ #pragma once + #include #include diff --git a/MbDCode/SparseVector.h b/MbDCode/SparseVector.h index bf08b53..cbde3f3 100644 --- a/MbDCode/SparseVector.h +++ b/MbDCode/SparseVector.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/SphSphJoint.cpp b/MbDCode/SphSphJoint.cpp new file mode 100644 index 0000000..f1fdfe9 --- /dev/null +++ b/MbDCode/SphSphJoint.cpp @@ -0,0 +1,28 @@ +#include "SphSphJoint.h" +#include "CREATE.h" +#include "DistanceConstraintIJ.h" +#include "System.h" + +using namespace MbD; + +MbD::SphSphJoint::SphSphJoint() +{ +} + +MbD::SphSphJoint::SphSphJoint(const char* str) : CompoundJoint(str) +{ +} + +void MbD::SphSphJoint::initializeGlobally() +{ + if (constraints->empty()) + { + auto distxyIJ = CREATE::With(frmI, frmJ); + distxyIJ->setConstant(distanceIJ); + addConstraint(distxyIJ); + this->root()->hasChanged = true; + } + else { + CompoundJoint::initializeGlobally(); + } +} diff --git a/MbDCode/SphSphJoint.h b/MbDCode/SphSphJoint.h new file mode 100644 index 0000000..5ac1728 --- /dev/null +++ b/MbDCode/SphSphJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "CompoundJoint.h" + +namespace MbD { + class SphSphJoint : public CompoundJoint + { + // + public: + SphSphJoint(); + SphSphJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/SphericalJoint.cpp b/MbDCode/SphericalJoint.cpp new file mode 100644 index 0000000..9b534a4 --- /dev/null +++ b/MbDCode/SphericalJoint.cpp @@ -0,0 +1,25 @@ +#include "SphericalJoint.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::SphericalJoint::SphericalJoint() +{ +} + +MbD::SphericalJoint::SphericalJoint(const char* str) : AtPointJoint(str) +{ +} + +void MbD::SphericalJoint::initializeGlobally() +{ + if (constraints->empty()) + { + createAtPointConstraints(); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/SphericalJoint.h b/MbDCode/SphericalJoint.h new file mode 100644 index 0000000..f1631a7 --- /dev/null +++ b/MbDCode/SphericalJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "AtPointJoint.h" + +namespace MbD { + class SphericalJoint : public AtPointJoint + { + // + public: + SphericalJoint(); + SphericalJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/Symbolic.h b/MbDCode/Symbolic.h index 1e09e23..f2370ea 100644 --- a/MbDCode/Symbolic.h +++ b/MbDCode/Symbolic.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/System.h b/MbDCode/System.h index 174f523..e22e679 100644 --- a/MbDCode/System.h +++ b/MbDCode/System.h @@ -7,6 +7,7 @@ *********************************************************************/ #pragma once + #include #include #include diff --git a/MbDCode/SystemSolver.h b/MbDCode/SystemSolver.h index c0e6efc..e5e820d 100644 --- a/MbDCode/SystemSolver.h +++ b/MbDCode/SystemSolver.h @@ -1,4 +1,5 @@ #pragma once + #include #include #include diff --git a/MbDCode/Translation.cpp b/MbDCode/Translation.cpp new file mode 100644 index 0000000..a141924 --- /dev/null +++ b/MbDCode/Translation.cpp @@ -0,0 +1,31 @@ +#include "Translation.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::Translation::Translation() +{ +} + +MbD::Translation::Translation(const char* str) +{ +} + +void MbD::Translation::initializeGlobally() +{ + if (constraints->empty()) { + initMotions(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2)); + this->root()->hasChanged = true; + } + else { + PrescribedMotion::initializeGlobally(); + } +} + +void MbD::Translation::initMotions() +{ +} diff --git a/MbDCode/Translation.h b/MbDCode/Translation.h new file mode 100644 index 0000000..a2ef129 --- /dev/null +++ b/MbDCode/Translation.h @@ -0,0 +1,17 @@ +#pragma once + +#include "PrescribedMotion.h" + +namespace MbD { + class Translation : public PrescribedMotion + { + // + public: + Translation(); + Translation(const char* str); + void initializeGlobally() override; + void initMotions() override; + + }; +} + diff --git a/MbDCode/TranslationConstraintIJ.cpp b/MbDCode/TranslationConstraintIJ.cpp index 16bf453..9b46470 100644 --- a/MbDCode/TranslationConstraintIJ.cpp +++ b/MbDCode/TranslationConstraintIJ.cpp @@ -63,6 +63,12 @@ void TranslationConstraintIJ::preVelIC() Item::preVelIC(); } +void MbD::TranslationConstraintIJ::simUpdateAll() +{ + riIeJeIe->simUpdateAll(); + Constraint::simUpdateAll(); +} + void TranslationConstraintIJ::preAccIC() { riIeJeIe->preAccIC(); diff --git a/MbDCode/TranslationConstraintIJ.h b/MbDCode/TranslationConstraintIJ.h index 6956ecf..09df81c 100644 --- a/MbDCode/TranslationConstraintIJ.h +++ b/MbDCode/TranslationConstraintIJ.h @@ -9,20 +9,23 @@ namespace MbD { //riIeJeIe public: TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); + + void calcPostDynCorrectorIteration() override; void initialize() override; - void initializeLocally() override; void initializeGlobally() override; + void initializeLocally() override; virtual void initriIeJeIe(); void postInput() override; - void calcPostDynCorrectorIteration() override; - void prePosIC()override; - ConstraintType type() override; void postPosICIteration() override; - void preVelIC() override; void preAccIC() override; + void prePosIC()override; + void preVelIC() override; + void simUpdateAll() override; + ConstraintType type() override; int axisI; std::shared_ptr riIeJeIe; + //ToDo: Use DispCompIecJecIe instead of DispCompIecJecKec }; } diff --git a/MbDCode/TranslationConstraintIqcJc.h b/MbDCode/TranslationConstraintIqcJc.h index f4db007..2282460 100644 --- a/MbDCode/TranslationConstraintIqcJc.h +++ b/MbDCode/TranslationConstraintIqcJc.h @@ -8,21 +8,20 @@ namespace MbD { //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI public: TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); - void initriIeJeIe() override; + + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; void calcPostDynCorrectorIteration() override; - void useEquationNumbers() override; + void initriIeJeIe() override; + void fillAccICIterError(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; - void fillAccICIterError(FColDsptr col) override; - void addToJointForceI(FColDsptr col) override; - void addToJointTorqueI(FColDsptr col) override; + void useEquationNumbers() override; - FRowDsptr pGpXI; - FRowDsptr pGpEI; - FMatDsptr ppGpXIpEI; - FMatDsptr ppGpEIpEI; + FRowDsptr pGpXI, pGpEI; + FMatDsptr ppGpXIpEI, ppGpEIpEI; int iqXI, iqEI; }; } diff --git a/MbDCode/TranslationConstraintIqcJqc.h b/MbDCode/TranslationConstraintIqcJqc.h index 6da8180..5646e17 100644 --- a/MbDCode/TranslationConstraintIqcJqc.h +++ b/MbDCode/TranslationConstraintIqcJqc.h @@ -8,20 +8,18 @@ namespace MbD { //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ public: TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); - void initriIeJeIe() override; + void calcPostDynCorrectorIteration() override; - void useEquationNumbers() override; + void initriIeJeIe() override; + void fillAccICIterError(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; - void fillAccICIterError(FColDsptr col) override; + void useEquationNumbers() override; - FRowDsptr pGpXJ; - FRowDsptr pGpEJ; - FMatDsptr ppGpEIpXJ; - FMatDsptr ppGpEIpEJ; - FMatDsptr ppGpEJpEJ; + FRowDsptr pGpXJ, pGpEJ; + FMatDsptr ppGpEIpXJ, ppGpEIpEJ, ppGpEJpEJ; int iqXJ = -1, iqEJ = -1; }; } diff --git a/MbDCode/TranslationConstraintIqctJqc.h b/MbDCode/TranslationConstraintIqctJqc.h index 8b19ba8..2b42825 100644 --- a/MbDCode/TranslationConstraintIqctJqc.h +++ b/MbDCode/TranslationConstraintIqctJqc.h @@ -8,12 +8,13 @@ namespace MbD { //pGpt ppGpXIpt ppGpEIpt ppGpXJpt ppGpEJpt ppGptpt public: TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); - void initriIeJeIe() override; - ConstraintType type() override; - void preVelIC() override; - void fillVelICError(FColDsptr col) override; - void preAccIC() override; + void fillAccICIterError(FColDsptr col) override; + void fillVelICError(FColDsptr col) override; + void initriIeJeIe() override; + void preAccIC() override; + void preVelIC() override; + ConstraintType type() override; double pGpt; FRowDsptr ppGpXIpt; diff --git a/MbDCode/TranslationalJoint.cpp b/MbDCode/TranslationalJoint.cpp new file mode 100644 index 0000000..deb6572 --- /dev/null +++ b/MbDCode/TranslationalJoint.cpp @@ -0,0 +1,30 @@ +#include "TranslationalJoint.h" +#include "System.h" +#include "DirectionCosineConstraintIJ.h" +#include "TranslationConstraintIJ.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::TranslationalJoint::TranslationalJoint() +{ +} + +MbD::TranslationalJoint::TranslationalJoint(const char* str) : InLineJoint(str) +{ +} + +void MbD::TranslationalJoint::initializeGlobally() +{ + if (constraints->empty()) + { + createInLineConstraints(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + InLineJoint::initializeGlobally(); + } +} diff --git a/MbDCode/TranslationalJoint.h b/MbDCode/TranslationalJoint.h new file mode 100644 index 0000000..6e10f93 --- /dev/null +++ b/MbDCode/TranslationalJoint.h @@ -0,0 +1,17 @@ +#pragma once + +#include "InLineJoint.h" + +namespace MbD { + class TranslationalJoint : public InLineJoint + { + // + public: + TranslationalJoint(); + TranslationalJoint(const char* str); + void initializeGlobally() override; + + + }; +} + diff --git a/MbDCode/UniversalJoint.cpp b/MbDCode/UniversalJoint.cpp new file mode 100644 index 0000000..5eaf56d --- /dev/null +++ b/MbDCode/UniversalJoint.cpp @@ -0,0 +1,26 @@ +#include "UniversalJoint.h" +#include "System.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::UniversalJoint::UniversalJoint() +{ +} + +MbD::UniversalJoint::UniversalJoint(const char* str) : AtPointJoint(str) +{ +} + +void MbD::UniversalJoint::initializeGlobally() +{ + if (constraints->empty()) + { + createAtPointConstraints(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 2)); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/UniversalJoint.h b/MbDCode/UniversalJoint.h new file mode 100644 index 0000000..e5d91e7 --- /dev/null +++ b/MbDCode/UniversalJoint.h @@ -0,0 +1,16 @@ +#pragma once + +#include "AtPointJoint.h" + +namespace MbD { + class UniversalJoint : public AtPointJoint + { + // + public: + UniversalJoint(); + UniversalJoint(const char* str); + void initializeGlobally() override; + + }; +} + diff --git a/MbDCode/Variable.h b/MbDCode/Variable.h index 677c0ea..f436f87 100644 --- a/MbDCode/Variable.h +++ b/MbDCode/Variable.h @@ -1,4 +1,5 @@ #pragma once + #include #include "Symbolic.h" diff --git a/MbDCode/VelSolver.h b/MbDCode/VelSolver.h index 8eed930..cec8302 100644 --- a/MbDCode/VelSolver.h +++ b/MbDCode/VelSolver.h @@ -1,4 +1,5 @@ #pragma once + #include #include "Solver.h" diff --git a/MbDCode/ZRotation.cpp b/MbDCode/ZRotation.cpp index c0c1cf2..be42cda 100644 --- a/MbDCode/ZRotation.cpp +++ b/MbDCode/ZRotation.cpp @@ -25,7 +25,7 @@ void ZRotation::initializeGlobally() this->root()->hasChanged = true; } else { - Joint::initializeGlobally(); + PrescribedMotion::initializeGlobally(); } } diff --git a/MbDCode/ZRotation.h b/MbDCode/ZRotation.h index e52252c..d607114 100644 --- a/MbDCode/ZRotation.h +++ b/MbDCode/ZRotation.h @@ -1,4 +1,5 @@ #pragma once + #include "PrescribedMotion.h" namespace MbD { @@ -9,7 +10,7 @@ namespace MbD { ZRotation(); ZRotation(const char* str); void initializeGlobally() override; - void initMotions(); + void initMotions() override; }; } diff --git a/MbDCode/ZTranslation.cpp b/MbDCode/ZTranslation.cpp new file mode 100644 index 0000000..dffff3b --- /dev/null +++ b/MbDCode/ZTranslation.cpp @@ -0,0 +1,29 @@ +#include "ZTranslation.h" +#include "CREATE.h" +#include "System.h" + +using namespace MbD; + +MbD::ZTranslation::ZTranslation() +{ +} + +MbD::ZTranslation::ZTranslation(const char* str) +{ +} + +void MbD::ZTranslation::initializeGlobally() +{ + if (constraints->empty()) { + initMotions(); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); + this->root()->hasChanged = true; + } + else { + PrescribedMotion::initializeGlobally(); + } +} + +void MbD::ZTranslation::initMotions() +{ +} diff --git a/MbDCode/ZTranslation.h b/MbDCode/ZTranslation.h new file mode 100644 index 0000000..5278246 --- /dev/null +++ b/MbDCode/ZTranslation.h @@ -0,0 +1,17 @@ +#pragma once + +#include "PrescribedMotion.h" + +namespace MbD { + class ZTranslation : public PrescribedMotion + { + // + public: + ZTranslation(); + ZTranslation(const char* str); + void initializeGlobally() override; + void initMotions() override; + + }; +} + diff --git a/MbDCode/typedef.h b/MbDCode/typedef.h index 04e106b..2a63d29 100644 --- a/MbDCode/typedef.h +++ b/MbDCode/typedef.h @@ -1,4 +1,5 @@ #pragma once + #include namespace MbD {