diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index 360f8b0..448bed9 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include #include @@ -64,1412 +64,1512 @@ using namespace MbD; -MbD::ASMTAssembly::ASMTAssembly() : ASMTSpatialContainer() +MbD::ASMTAssembly::ASMTAssembly() + : ASMTSpatialContainer() { - times = std::make_shared>(); + times = std::make_shared>(); } std::shared_ptr MbD::ASMTAssembly::With() { - auto asmt = std::make_shared(); - asmt->initialize(); - return asmt; + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; } void MbD::ASMTAssembly::runSinglePendulumSuperSimplified() { - //In this version we skip declaration of variables that don't need as they use default values. - auto assembly = CREATE::With(); + // In this version we skip declaration of variables that don't need as they use default values. + auto assembly = CREATE::With(); - assembly->setName("Assembly1"); + assembly->setName("Assembly1"); - auto mkr = CREATE::With(); - mkr->setName("Marker1"); - assembly->addMarker(mkr); + auto mkr = CREATE::With(); + mkr->setName("Marker1"); + assembly->addMarker(mkr); - auto part = CREATE::With(); - part->setName("Part1"); - part->setPosition3D(-0.1, -0.1, -0.1); - assembly->addPart(part); + auto part = CREATE::With(); + part->setName("Part1"); + part->setPosition3D(-0.1, -0.1, -0.1); + assembly->addPart(part); - mkr = CREATE::With(); - mkr->setName("Marker1"); - mkr->setPosition3D(0.1, 0.1, 0.1); - part->addMarker(mkr); + mkr = CREATE::With(); + mkr->setName("Marker1"); + mkr->setPosition3D(0.1, 0.1, 0.1); + part->addMarker(mkr); - auto joint = CREATE::With(); - joint->setName("Joint1"); - joint->setMarkerI("/Assembly1/Marker1"); - joint->setMarkerJ("/Assembly1/Part1/Marker1"); - assembly->addJoint(joint); + auto joint = CREATE::With(); + joint->setName("Joint1"); + joint->setMarkerI("/Assembly1/Marker1"); + joint->setMarkerJ("/Assembly1/Part1/Marker1"); + assembly->addJoint(joint); - auto simulationParameters = CREATE::With(); - simulationParameters->settstart(0.0); - simulationParameters->settend(0.0); //tstart == tend Initial Conditions only. - simulationParameters->sethmin(1.0e-9); - simulationParameters->sethmax(1.0); - simulationParameters->sethout(0.04); - simulationParameters->seterrorTol(1.0e-6); - assembly->setSimulationParameters(simulationParameters); + auto simulationParameters = CREATE::With(); + simulationParameters->settstart(0.0); + simulationParameters->settend(0.0); // tstart == tend Initial Conditions only. + simulationParameters->sethmin(1.0e-9); + simulationParameters->sethmax(1.0); + simulationParameters->sethout(0.04); + simulationParameters->seterrorTol(1.0e-6); + assembly->setSimulationParameters(simulationParameters); - assembly->runKINEMATIC(); + assembly->runKINEMATIC(); } void MbD::ASMTAssembly::runSinglePendulumSuperSimplified2() { - //In this version we skip declaration of variables that don't need as they use default values. - auto assembly = CREATE::With(); - assembly->setName("OndselAssembly"); - - auto mkr = CREATE::With(); - mkr->setName("marker1"); - assembly->addMarker(mkr); - - auto part = CREATE::With(); - part->setName("part1"); - assembly->addPart(part); - - auto marker1 = CREATE::With(); - marker1->setName("FixingMarker"); - part->addMarker(marker1); - - auto marker2 = CREATE::With(); - marker2->setName("marker2"); - marker2->setPosition3D(20.0, 10.0, 0.0); - part->addMarker(marker2); - - auto part2 = CREATE::With(); - part2->setName("part2"); - part2->setPosition3D(20.0, 10.0, 0.0); - assembly->addPart(part2); - - auto marker3 = CREATE::With(); - marker3->setName("marker2"); - marker3->setPosition3D(50.0, 10.0, 0.0); - part2->addMarker(marker3); - - /*Ground joint*/ - auto joint = CREATE::With(); - joint->setName("Joint1"); - joint->setMarkerI("/OndselAssembly/marker1"); - joint->setMarkerJ("/OndselAssembly/part1/FixingMarker"); - assembly->addJoint(joint); - - auto joint2 = CREATE::With(); - joint2->setName("Joint2"); - joint2->setMarkerI("/OndselAssembly/part1/marker2"); - joint2->setMarkerJ("/OndselAssembly/part2/marker2"); - assembly->addJoint(joint2); - - auto simulationParameters = CREATE::With(); - simulationParameters->settstart(0.0); - simulationParameters->settend(0.0); //tstart == tend Initial Conditions only. - simulationParameters->sethmin(1.0e-9); - simulationParameters->sethmax(1.0); - simulationParameters->sethout(0.04); - simulationParameters->seterrorTol(1.0e-6); - assembly->setSimulationParameters(simulationParameters); - - assembly->runKINEMATIC(); + // In this version we skip declaration of variables that don't need as they use default values. + auto assembly = CREATE::With(); + assembly->setName("OndselAssembly"); + + auto mkr = CREATE::With(); + mkr->setName("marker1"); + assembly->addMarker(mkr); + + auto part = CREATE::With(); + part->setName("part1"); + assembly->addPart(part); + + auto marker1 = CREATE::With(); + marker1->setName("FixingMarker"); + part->addMarker(marker1); + + auto marker2 = CREATE::With(); + marker2->setName("marker2"); + marker2->setPosition3D(20.0, 10.0, 0.0); + part->addMarker(marker2); + + auto part2 = CREATE::With(); + part2->setName("part2"); + part2->setPosition3D(20.0, 10.0, 0.0); + assembly->addPart(part2); + + auto marker3 = CREATE::With(); + marker3->setName("marker2"); + marker3->setPosition3D(50.0, 10.0, 0.0); + part2->addMarker(marker3); + + /*Ground joint*/ + auto joint = CREATE::With(); + joint->setName("Joint1"); + joint->setMarkerI("/OndselAssembly/marker1"); + joint->setMarkerJ("/OndselAssembly/part1/FixingMarker"); + assembly->addJoint(joint); + + auto joint2 = CREATE::With(); + joint2->setName("Joint2"); + joint2->setMarkerI("/OndselAssembly/part1/marker2"); + joint2->setMarkerJ("/OndselAssembly/part2/marker2"); + assembly->addJoint(joint2); + + auto simulationParameters = CREATE::With(); + simulationParameters->settstart(0.0); + simulationParameters->settend(0.0); // tstart == tend Initial Conditions only. + simulationParameters->sethmin(1.0e-9); + simulationParameters->sethmax(1.0); + simulationParameters->sethout(0.04); + simulationParameters->seterrorTol(1.0e-6); + assembly->setSimulationParameters(simulationParameters); + + assembly->runKINEMATIC(); } void MbD::ASMTAssembly::runSinglePendulumSimplified() { - auto assembly = CREATE::With(); - - assembly->setNotes(""); - assembly->setName("Assembly1"); - assembly->setPosition3D(0, 0, 0); - assembly->setRotationMatrix( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - assembly->setVelocity3D(0, 0, 0); - assembly->setOmega3D(0, 0, 0); - - auto massMarker = ASMTPrincipalMassMarker::With(); - massMarker->setMass(0.0); - massMarker->setDensity(0.0); - massMarker->setMomentOfInertias(0, 0, 0); - massMarker->setPosition3D(0, 0, 0); - massMarker->setRotationMatrix( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - assembly->setPrincipalMassMarker(massMarker); - - auto mkr = CREATE::With(); - mkr->setName("Marker1"); - mkr->setPosition3D(0, 0, 0); - mkr->setRotationMatrix( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - assembly->addMarker(mkr); - - auto part = CREATE::With(); - part->setName("Part1"); - part->setPosition3D(-0.1, -0.1, -0.1); - part->setRotationMatrix( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - part->setVelocity3D(0, 0, 0); - part->setOmega3D(0, 0, 0); - assembly->addPart(part); - - massMarker = ASMTPrincipalMassMarker::With(); - massMarker->setMass(0.2); - massMarker->setDensity(10.0); - massMarker->setMomentOfInertias(8.3333333333333e-4, 0.016833333333333, 0.017333333333333); - massMarker->setPosition3D(0.5, 0.1, 0.05); - massMarker->setRotationMatrix( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - part->setPrincipalMassMarker(massMarker); - - mkr = CREATE::With(); - mkr->setName("Marker1"); - mkr->setPosition3D(0.1, 0.1, 0.1); - mkr->setRotationMatrix( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - part->addMarker(mkr); - - auto joint = CREATE::With(); - joint->setName("Joint1"); - joint->setMarkerI("/Assembly1/Marker1"); - joint->setMarkerJ("/Assembly1/Part1/Marker1"); - assembly->addJoint(joint); - - auto motion = CREATE::With(); - motion->setName("Motion1"); - motion->setMotionJoint("/Assembly1/Joint1"); - motion->setRotationZ("0.0"); - assembly->addMotion(motion); - - auto constantGravity = CREATE::With(); - constantGravity->setg(0.0, 0.0, 0.0); - assembly->setConstantGravity(constantGravity); - - auto simulationParameters = CREATE::With(); - simulationParameters->settstart(0.0); - simulationParameters->settend(0.0); //tstart == tend Initial Conditions only. - simulationParameters->sethmin(1.0e-9); - simulationParameters->sethmax(1.0); - simulationParameters->sethout(0.04); - simulationParameters->seterrorTol(1.0e-6); - assembly->setSimulationParameters(simulationParameters); - - assembly->runKINEMATIC(); + auto assembly = CREATE::With(); + + assembly->setNotes(""); + assembly->setName("Assembly1"); + assembly->setPosition3D(0, 0, 0); + assembly->setRotationMatrix(1, 0, 0, 0, 1, 0, 0, 0, 1); + assembly->setVelocity3D(0, 0, 0); + assembly->setOmega3D(0, 0, 0); + + auto massMarker = ASMTPrincipalMassMarker::With(); + massMarker->setMass(0.0); + massMarker->setDensity(0.0); + massMarker->setMomentOfInertias(0, 0, 0); + massMarker->setPosition3D(0, 0, 0); + massMarker->setRotationMatrix(1, 0, 0, 0, 1, 0, 0, 0, 1); + assembly->setPrincipalMassMarker(massMarker); + + auto mkr = CREATE::With(); + mkr->setName("Marker1"); + mkr->setPosition3D(0, 0, 0); + mkr->setRotationMatrix(1, 0, 0, 0, 1, 0, 0, 0, 1); + assembly->addMarker(mkr); + + auto part = CREATE::With(); + part->setName("Part1"); + part->setPosition3D(-0.1, -0.1, -0.1); + part->setRotationMatrix(1, 0, 0, 0, 1, 0, 0, 0, 1); + part->setVelocity3D(0, 0, 0); + part->setOmega3D(0, 0, 0); + assembly->addPart(part); + + massMarker = ASMTPrincipalMassMarker::With(); + massMarker->setMass(0.2); + massMarker->setDensity(10.0); + massMarker->setMomentOfInertias(8.3333333333333e-4, 0.016833333333333, 0.017333333333333); + massMarker->setPosition3D(0.5, 0.1, 0.05); + massMarker->setRotationMatrix(1, 0, 0, 0, 1, 0, 0, 0, 1); + part->setPrincipalMassMarker(massMarker); + + mkr = CREATE::With(); + mkr->setName("Marker1"); + mkr->setPosition3D(0.1, 0.1, 0.1); + mkr->setRotationMatrix(1, 0, 0, 0, 1, 0, 0, 0, 1); + part->addMarker(mkr); + + auto joint = CREATE::With(); + joint->setName("Joint1"); + joint->setMarkerI("/Assembly1/Marker1"); + joint->setMarkerJ("/Assembly1/Part1/Marker1"); + assembly->addJoint(joint); + + auto motion = CREATE::With(); + motion->setName("Motion1"); + motion->setMotionJoint("/Assembly1/Joint1"); + motion->setRotationZ("0.0"); + assembly->addMotion(motion); + + auto constantGravity = CREATE::With(); + constantGravity->setg(0.0, 0.0, 0.0); + assembly->setConstantGravity(constantGravity); + + auto simulationParameters = CREATE::With(); + simulationParameters->settstart(0.0); + simulationParameters->settend(0.0); // tstart == tend Initial Conditions only. + simulationParameters->sethmin(1.0e-9); + simulationParameters->sethmax(1.0); + simulationParameters->sethout(0.04); + simulationParameters->seterrorTol(1.0e-6); + assembly->setSimulationParameters(simulationParameters); + + assembly->runKINEMATIC(); } void MbD::ASMTAssembly::runSinglePendulum() { - auto assembly = CREATE::With(); - std::string str = ""; - assembly->setNotes(str); - str = "Assembly1"; - assembly->setName(str); - auto pos3D = std::make_shared>(ListD{ 0, 0, 0 }); - assembly->setPosition3D(pos3D); - auto rotMat = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - assembly->setRotationMatrix(rotMat); - auto vel3D = std::make_shared>(ListD{ 0, 0, 0 }); - assembly->setVelocity3D(vel3D); - auto ome3D = std::make_shared>(ListD{ 0, 0, 0 }); - assembly->setOmega3D(ome3D); - // - auto massMarker = ASMTPrincipalMassMarker::With(); - massMarker->setMass(0.0); - massMarker->setDensity(0.0); - auto aJ = std::make_shared>(ListD{ 0, 0, 0 }); - massMarker->setMomentOfInertias(aJ); - pos3D = std::make_shared>(ListD{ 0, 0, 0 }); - massMarker->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - massMarker->setRotationMatrix(rotMat); - assembly->setPrincipalMassMarker(massMarker); - // - auto mkr = CREATE::With(); - str = "Marker1"; - mkr->setName(str); - pos3D = std::make_shared>(ListD{ 0, 0, 0 }); - mkr->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - mkr->setRotationMatrix(rotMat); - assembly->addMarker(mkr); - // - auto part = CREATE::With(); - str = "Part1"; - part->setName(str); - pos3D = std::make_shared>(ListD{ -0.1, -0.1, -0.1 }); - part->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - part->setRotationMatrix(rotMat); - vel3D = std::make_shared>(ListD{ 0, 0, 0 }); - part->setVelocity3D(vel3D); - ome3D = std::make_shared>(ListD{ 0, 0, 0 }); - part->setOmega3D(ome3D); - assembly->addPart(part); - // - massMarker = ASMTPrincipalMassMarker::With(); - massMarker->setMass(0.2); - massMarker->setDensity(10.0); - aJ = std::make_shared>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 }); - massMarker->setMomentOfInertias(aJ); - pos3D = std::make_shared>(ListD{ 0.5, 0.1, 0.05 }); - massMarker->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - massMarker->setRotationMatrix(rotMat); - part->setPrincipalMassMarker(massMarker); - // - mkr = CREATE::With(); - str = "Marker1"; - mkr->setName(str); - pos3D = std::make_shared>(ListD{ 0.1, 0.1, 0.1 }); - mkr->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - mkr->setRotationMatrix(rotMat); - part->addMarker(mkr); - // - auto joint = CREATE::With(); - str = "Joint1"; - joint->setName(str); - str = "/Assembly1/Marker1"; - joint->setMarkerI(str); - str = "/Assembly1/Part1/Marker1"; - joint->setMarkerJ(str); - assembly->addJoint(joint); - // - auto motion = CREATE::With(); - str = "Motion1"; - motion->setName(str); - str = "/Assembly1/Joint1"; - motion->setMotionJoint(str); - str = "0.0"; - motion->setRotationZ(str); - assembly->addMotion(motion); - // - auto constantGravity = CREATE::With(); - auto gAcceleration = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); - constantGravity->setg(gAcceleration); - assembly->setConstantGravity(constantGravity); - // - auto simulationParameters = CREATE::With(); - simulationParameters->settstart(0.0); - simulationParameters->settend(0.0); //tstart == tend Initial Conditions only. - simulationParameters->sethmin(1.0e-9); - simulationParameters->sethmax(1.0); - simulationParameters->sethout(0.04); - simulationParameters->seterrorTol(1.0e-6); - assembly->setSimulationParameters(simulationParameters); - // - assembly->runKINEMATIC(); + auto assembly = CREATE::With(); + std::string str = ""; + assembly->setNotes(str); + str = "Assembly1"; + assembly->setName(str); + auto pos3D = std::make_shared>(ListD {0, 0, 0}); + assembly->setPosition3D(pos3D); + auto rotMat = std::make_shared>(ListListD {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}); + assembly->setRotationMatrix(rotMat); + auto vel3D = std::make_shared>(ListD {0, 0, 0}); + assembly->setVelocity3D(vel3D); + auto ome3D = std::make_shared>(ListD {0, 0, 0}); + assembly->setOmega3D(ome3D); + // + auto massMarker = ASMTPrincipalMassMarker::With(); + massMarker->setMass(0.0); + massMarker->setDensity(0.0); + auto aJ = std::make_shared>(ListD {0, 0, 0}); + massMarker->setMomentOfInertias(aJ); + pos3D = std::make_shared>(ListD {0, 0, 0}); + massMarker->setPosition3D(pos3D); + rotMat = std::make_shared>(ListListD {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}); + massMarker->setRotationMatrix(rotMat); + assembly->setPrincipalMassMarker(massMarker); + // + auto mkr = CREATE::With(); + str = "Marker1"; + mkr->setName(str); + pos3D = std::make_shared>(ListD {0, 0, 0}); + mkr->setPosition3D(pos3D); + rotMat = std::make_shared>(ListListD {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}); + mkr->setRotationMatrix(rotMat); + assembly->addMarker(mkr); + // + auto part = CREATE::With(); + str = "Part1"; + part->setName(str); + pos3D = std::make_shared>(ListD {-0.1, -0.1, -0.1}); + part->setPosition3D(pos3D); + rotMat = std::make_shared>(ListListD {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}); + part->setRotationMatrix(rotMat); + vel3D = std::make_shared>(ListD {0, 0, 0}); + part->setVelocity3D(vel3D); + ome3D = std::make_shared>(ListD {0, 0, 0}); + part->setOmega3D(ome3D); + assembly->addPart(part); + // + massMarker = ASMTPrincipalMassMarker::With(); + massMarker->setMass(0.2); + massMarker->setDensity(10.0); + aJ = std::make_shared>( + ListD {8.3333333333333e-4, 0.016833333333333, 0.017333333333333}); + massMarker->setMomentOfInertias(aJ); + pos3D = std::make_shared>(ListD {0.5, 0.1, 0.05}); + massMarker->setPosition3D(pos3D); + rotMat = std::make_shared>(ListListD {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}); + massMarker->setRotationMatrix(rotMat); + part->setPrincipalMassMarker(massMarker); + // + mkr = CREATE::With(); + str = "Marker1"; + mkr->setName(str); + pos3D = std::make_shared>(ListD {0.1, 0.1, 0.1}); + mkr->setPosition3D(pos3D); + rotMat = std::make_shared>(ListListD {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}); + mkr->setRotationMatrix(rotMat); + part->addMarker(mkr); + // + auto joint = CREATE::With(); + str = "Joint1"; + joint->setName(str); + str = "/Assembly1/Marker1"; + joint->setMarkerI(str); + str = "/Assembly1/Part1/Marker1"; + joint->setMarkerJ(str); + assembly->addJoint(joint); + // + auto motion = CREATE::With(); + str = "Motion1"; + motion->setName(str); + str = "/Assembly1/Joint1"; + motion->setMotionJoint(str); + str = "0.0"; + motion->setRotationZ(str); + assembly->addMotion(motion); + // + auto constantGravity = CREATE::With(); + auto gAcceleration = std::make_shared>(ListD {0.0, 0.0, 0.0}); + constantGravity->setg(gAcceleration); + assembly->setConstantGravity(constantGravity); + // + auto simulationParameters = CREATE::With(); + simulationParameters->settstart(0.0); + simulationParameters->settend(0.0); // tstart == tend Initial Conditions only. + simulationParameters->sethmin(1.0e-9); + simulationParameters->sethmax(1.0); + simulationParameters->sethout(0.04); + simulationParameters->seterrorTol(1.0e-6); + assembly->setSimulationParameters(simulationParameters); + // + assembly->runKINEMATIC(); } std::shared_ptr MbD::ASMTAssembly::assemblyFromFile(const char* fileName) { - std::ifstream stream(fileName); - if (stream.fail()) { - throw std::invalid_argument("File not found."); - } - std::string line; - std::vector lines; - while (std::getline(stream, line)) { - lines.push_back(line); - } - auto assembly = ASMTAssembly::With(); - auto str = assembly->popOffTop(lines); - bool bool1 = str == "freeCAD: 3D CAD with Motion Simulation by askoh.com"; - bool bool2 = str == "OndselSolver"; - assert(bool1 || bool2); - assert(assembly->readStringOffTop(lines) == "Assembly"); - assembly->setFilename(fileName); - assembly->parseASMT(lines); - return assembly; + std::ifstream stream(fileName); + if (stream.fail()) { + throw std::invalid_argument("File not found."); + } + std::string line; + std::vector lines; + while (std::getline(stream, line)) { + lines.push_back(line); + } + auto assembly = ASMTAssembly::With(); + auto str = assembly->popOffTop(lines); + bool bool1 = str == "freeCAD: 3D CAD with Motion Simulation by askoh.com"; + bool bool2 = str == "OndselSolver"; + assert(bool1 || bool2); + assert(assembly->readStringOffTop(lines) == "Assembly"); + assembly->setFilename(fileName); + assembly->parseASMT(lines); + return assembly; } void MbD::ASMTAssembly::runFile(const char* fileName) { - std::ifstream stream(fileName); - if (stream.fail()) { - throw std::invalid_argument("File not found."); - } - std::string line; - std::vector lines; - while (std::getline(stream, line)) { - lines.push_back(line); - } - bool bool1 = lines[0] == "freeCAD: 3D CAD with Motion Simulation by askoh.com"; - bool bool2 = lines[0] == "OndselSolver"; - assert(bool1 || bool2); - lines.erase(lines.begin()); - - if (lines[0] == "Assembly") { - lines.erase(lines.begin()); - auto assembly = CREATE::With(); - assembly->setFilename(fileName); - assembly->parseASMT(lines); - assembly->runKINEMATIC(); - } + std::ifstream stream(fileName); + if (stream.fail()) { + throw std::invalid_argument("File not found."); + } + std::string line; + std::vector lines; + while (std::getline(stream, line)) { + lines.push_back(line); + } + bool bool1 = lines[0] == "freeCAD: 3D CAD with Motion Simulation by askoh.com"; + bool bool2 = lines[0] == "OndselSolver"; + assert(bool1 || bool2); + lines.erase(lines.begin()); + + if (lines[0] == "Assembly") { + lines.erase(lines.begin()); + auto assembly = CREATE::With(); + assembly->setFilename(fileName); + assembly->parseASMT(lines); + assembly->runKINEMATIC(); + } } void MbD::ASMTAssembly::runDraggingLogTest() { - runDraggingTest(); - auto assembly = ASMTAssembly::assemblyFromFile("runPreDrag.asmt"); - assembly->runDraggingLog("dragging.log"); + runDraggingTest(); + auto assembly = ASMTAssembly::assemblyFromFile("runPreDrag.asmt"); + assembly->runDraggingLog("dragging.log"); } void MbD::ASMTAssembly::runDraggingLogTest2() { - runDraggingTest2(); - auto assembly = ASMTAssembly::assemblyFromFile("runPreDrag.asmt"); - assembly->runDraggingLog("dragging.log"); + runDraggingTest2(); + auto assembly = ASMTAssembly::assemblyFromFile("runPreDrag.asmt"); + assembly->runDraggingLog("dragging.log"); } void MbD::ASMTAssembly::runDraggingLogTest3() { - runDraggingTest3(); - auto assembly = ASMTAssembly::assemblyFromFile("runPreDrag.asmt"); - assembly->runDraggingLog("dragging.log"); + runDraggingTest3(); + auto assembly = ASMTAssembly::assemblyFromFile("runPreDrag.asmt"); + assembly->runDraggingLog("dragging.log"); } void MbD::ASMTAssembly::runDraggingTest() { - //auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt"); - auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt"); - assembly->setDebug(true); - - auto limit1 = ASMTRotationLimit::With(); - limit1->setName("Limit1"); - limit1->setMarkerI("/Assembly1/Marker2"); - limit1->setMarkerJ("/Assembly1/Part1/Marker1"); - limit1->settype("=>"); - limit1->setlimit("30.0*pi/180.0"); - limit1->settol("1.0e-9"); - assembly->addLimit(limit1); - - auto limit2 = ASMTTranslationLimit::With(); - limit2->setName("Limit2"); - limit2->setMarkerI("/Assembly1/Part3/Marker2"); - limit2->setMarkerJ("/Assembly1/Marker1"); - limit2->settype("=<"); - limit2->setlimit("1.2"); - limit2->settol("1.0e-9"); - assembly->addLimit(limit2); - - auto& dragPart = assembly->parts->at(0); - auto dragParts = std::make_shared>>(); - dragParts->push_back(dragPart); - assembly->runPreDrag(); //Do this before first drag - FColDsptr pos3D, delta; - pos3D = dragPart->position3D; - delta = std::make_shared>(ListD{ 0.1, 0.2, 0.3 }); - dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta)); - assembly->runDragStep(dragParts); - pos3D = dragPart->position3D; - delta = std::make_shared>(ListD{ 0.3, 0.2, 0.1 }); - dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta)); - assembly->runDragStep(dragParts); - assembly->runPostDrag(); //Do this after last drag + // auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt"); + auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt"); + assembly->setDebug(true); + + auto limit1 = ASMTRotationLimit::With(); + limit1->setName("Limit1"); + limit1->setMarkerI("/Assembly1/Marker2"); + limit1->setMarkerJ("/Assembly1/Part1/Marker1"); + limit1->settype("=>"); + limit1->setlimit("30.0*pi/180.0"); + limit1->settol("1.0e-9"); + assembly->addLimit(limit1); + + auto limit2 = ASMTTranslationLimit::With(); + limit2->setName("Limit2"); + limit2->setMarkerI("/Assembly1/Part3/Marker2"); + limit2->setMarkerJ("/Assembly1/Marker1"); + limit2->settype("=<"); + limit2->setlimit("1.2"); + limit2->settol("1.0e-9"); + assembly->addLimit(limit2); + + auto& dragPart = assembly->parts->at(0); + auto dragParts = std::make_shared>>(); + dragParts->push_back(dragPart); + assembly->runPreDrag(); // Do this before first drag + FColDsptr pos3D, delta; + pos3D = dragPart->position3D; + delta = std::make_shared>(ListD {0.1, 0.2, 0.3}); + dragPart->setPosition3D(pos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + pos3D = dragPart->position3D; + delta = std::make_shared>(ListD {0.3, 0.2, 0.1}); + dragPart->setPosition3D(pos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + assembly->runPostDrag(); // Do this after last drag } void MbD::ASMTAssembly::runDraggingTest2() { - //auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt"); - auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt"); - assembly->setDebug(true); - - auto limit1 = ASMTRotationLimit::With(); - limit1->setName("Limit1"); - limit1->setmotionJoint("/Assembly1/Joint1"); - limit1->settype("=>"); - limit1->setlimit("0.0*pi/180.0"); - limit1->settol("1.0e-9"); - assembly->addLimit(limit1); - - auto limit2 = ASMTTranslationLimit::With(); - limit2->setName("Limit2"); - limit2->setmotionJoint("/Assembly1/Joint4"); - limit2->settype("=<"); - limit2->setlimit("0.0"); - limit2->settol("1.0e-9"); - assembly->addLimit(limit2); - - assembly->outputFile("assembly.asmt"); - - auto& dragPart = assembly->parts->at(0); - auto dragParts = std::make_shared>>(); - dragParts->push_back(dragPart); - assembly->runPreDrag(); //Do this before first drag - FColDsptr pos3D, delta; - pos3D = dragPart->position3D; - delta = std::make_shared>(ListD{ 0.1, 0.2, 0.3 }); - dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta)); - assembly->runDragStep(dragParts); - pos3D = dragPart->position3D; - delta = std::make_shared>(ListD{ 0.3, 0.2, 0.1 }); - dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta)); - assembly->runDragStep(dragParts); - assembly->runPostDrag(); //Do this after last drag + // auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt"); + auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt"); + assembly->setDebug(true); + + auto limit1 = ASMTRotationLimit::With(); + limit1->setName("Limit1"); + limit1->setmotionJoint("/Assembly1/Joint1"); + limit1->settype("=>"); + limit1->setlimit("0.0*pi/180.0"); + limit1->settol("1.0e-9"); + assembly->addLimit(limit1); + + auto limit2 = ASMTTranslationLimit::With(); + limit2->setName("Limit2"); + limit2->setmotionJoint("/Assembly1/Joint4"); + limit2->settype("=<"); + limit2->setlimit("0.0"); + limit2->settol("1.0e-9"); + assembly->addLimit(limit2); + + assembly->outputFile("assembly.asmt"); + + auto& dragPart = assembly->parts->at(0); + auto dragParts = std::make_shared>>(); + dragParts->push_back(dragPart); + assembly->runPreDrag(); // Do this before first drag + FColDsptr pos3D, delta; + pos3D = dragPart->position3D; + delta = std::make_shared>(ListD {0.1, 0.2, 0.3}); + dragPart->setPosition3D(pos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + pos3D = dragPart->position3D; + delta = std::make_shared>(ListD {0.3, 0.2, 0.1}); + dragPart->setPosition3D(pos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + assembly->runPostDrag(); // Do this after last drag } void MbD::ASMTAssembly::runDraggingTest3() { - auto assembly = ASMTAssembly::assemblyFromFile("../testapp/rackPinion3.asmt"); - assembly->setDebug(true); - auto dragPart = assembly->partNamed("/OndselAssembly/rackPinion#Box"); - auto rotPart = assembly->partNamed("/OndselAssembly/rackPinion#Cylinder"); - auto dragParts = std::make_shared>>(); - dragParts->push_back(dragPart); - FColDsptr dragPos3D, rotPos3D, delta; - FMatDsptr rotMat; - assembly->runPreDrag(); //Do this before first drag - dragPos3D = dragPart->position3D; - rotPos3D = rotPart->position3D; - rotMat = rotPart->rotationMatrix; - delta = std::make_shared>(ListD{ 0.5, 0.0, 0.0 }); - dragPart->updateMbDFromPosition3D(dragPos3D->plusFullColumn(delta)); - assembly->runDragStep(dragParts); - dragPos3D = dragPart->position3D; - rotPos3D = rotPart->position3D; - rotMat = rotPart->rotationMatrix; - delta = std::make_shared>(ListD{ 0.5, 0.0, 0.0 }); - dragPart->updateMbDFromPosition3D(dragPos3D->plusFullColumn(delta)); - assembly->runDragStep(dragParts); - dragPos3D = dragPart->position3D; - rotPos3D = rotPart->position3D; - rotMat = rotPart->rotationMatrix; - assembly->runPostDrag(); //Do this after last drag - dragPos3D = dragPart->position3D; - rotPos3D = rotPart->position3D; - rotMat = rotPart->rotationMatrix; + auto assembly = ASMTAssembly::assemblyFromFile("../testapp/rackPinion3.asmt"); + assembly->setDebug(true); + auto dragPart = assembly->partNamed("/OndselAssembly/rackPinion#Box"); + auto rotPart = assembly->partNamed("/OndselAssembly/rackPinion#Cylinder"); + auto dragParts = std::make_shared>>(); + dragParts->push_back(dragPart); + FColDsptr dragPos3D, rotPos3D, delta; + FMatDsptr rotMat; + assembly->runPreDrag(); // Do this before first drag + dragPos3D = dragPart->position3D; + rotPos3D = rotPart->position3D; + rotMat = rotPart->rotationMatrix; + delta = std::make_shared>(ListD {0.5, 0.0, 0.0}); + dragPart->setPosition3D(dragPos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + dragPos3D = dragPart->position3D; + rotPos3D = rotPart->position3D; + rotMat = rotPart->rotationMatrix; + delta = std::make_shared>(ListD {0.5, 0.0, 0.0}); + dragPart->setPosition3D(dragPos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + dragPos3D = dragPart->position3D; + rotPos3D = rotPart->position3D; + rotMat = rotPart->rotationMatrix; + assembly->runPostDrag(); // Do this after last drag + dragPos3D = dragPart->position3D; + rotPos3D = rotPart->position3D; + rotMat = rotPart->rotationMatrix; } void MbD::ASMTAssembly::readWriteFile(const char* fileName) { - std::ifstream stream(fileName); - if (stream.fail()) { - throw std::invalid_argument("File not found."); - } - std::string line; - std::vector lines; - while (std::getline(stream, line)) { - lines.push_back(line); - } - bool bool1 = lines[0] == "freeCAD: 3D CAD with Motion Simulation by askoh.com"; - bool bool2 = lines[0] == "OndselSolver"; - assert(bool1 || bool2); - lines.erase(lines.begin()); - - if (lines[0] == "Assembly") { - lines.erase(lines.begin()); - auto assembly = CREATE::With(); - assembly->parseASMT(lines); - assembly->runKINEMATIC(); - assembly->outputFile("assembly.asmt"); - ASMTAssembly::runFile("assembly.asmt"); - } + std::ifstream stream(fileName); + if (stream.fail()) { + throw std::invalid_argument("File not found."); + } + std::string line; + std::vector lines; + while (std::getline(stream, line)) { + lines.push_back(line); + } + bool bool1 = lines[0] == "freeCAD: 3D CAD with Motion Simulation by askoh.com"; + bool bool2 = lines[0] == "OndselSolver"; + assert(bool1 || bool2); + lines.erase(lines.begin()); + + if (lines[0] == "Assembly") { + lines.erase(lines.begin()); + auto assembly = CREATE::With(); + assembly->parseASMT(lines); + assembly->runKINEMATIC(); + assembly->outputFile("assembly.asmt"); + ASMTAssembly::runFile("assembly.asmt"); + } } void MbD::ASMTAssembly::initialize() { - ASMTSpatialContainer::initialize(); - times = std::make_shared>(); + ASMTSpatialContainer::initialize(); + times = std::make_shared>(); } ASMTAssembly* MbD::ASMTAssembly::root() { - return this; + return this; } void MbD::ASMTAssembly::setNotes(std::string str) { - notes = str; + notes = str; } void MbD::ASMTAssembly::parseASMT(std::vector& lines) { - readNotes(lines); - readName(lines); - readPosition3D(lines); - readRotationMatrix(lines); - readVelocity3D(lines); - readOmega3D(lines); - initprincipalMassMarker(); - readRefPoints(lines); - readRefCurves(lines); - readRefSurfaces(lines); - readParts(lines); - readKinematicIJs(lines); - readConstraintSets(lines); - readForcesTorques(lines); - readConstantGravity(lines); - readSimulationParameters(lines); - readAnimationParameters(lines); - readTimeSeries(lines); - readAssemblySeries(lines); - readPartSeriesMany(lines); - readJointSeriesMany(lines); - readMotionSeriesMany(lines); + readNotes(lines); + readName(lines); + readPosition3D(lines); + readRotationMatrix(lines); + readVelocity3D(lines); + readOmega3D(lines); + initprincipalMassMarker(); + readRefPoints(lines); + readRefCurves(lines); + readRefSurfaces(lines); + readParts(lines); + readKinematicIJs(lines); + readConstraintSets(lines); + readForcesTorques(lines); + readConstantGravity(lines); + readSimulationParameters(lines); + readAnimationParameters(lines); + readTimeSeries(lines); + readAssemblySeries(lines); + readPartSeriesMany(lines); + readJointSeriesMany(lines); + readMotionSeriesMany(lines); } void MbD::ASMTAssembly::readNotes(std::vector& lines) { - assert(lines[0] == "\tNotes"); - lines.erase(lines.begin()); - notes = readString(lines[0]); - lines.erase(lines.begin()); + assert(lines[0] == "\tNotes"); + lines.erase(lines.begin()); + notes = readString(lines[0]); + lines.erase(lines.begin()); } void MbD::ASMTAssembly::readParts(std::vector& lines) { - assert(lines[0] == "\tParts"); - lines.erase(lines.begin()); - parts->clear(); - auto it = std::find(lines.begin(), lines.end(), "\tKinematicIJs"); - std::vector partsLines(lines.begin(), it); - while (!partsLines.empty()) { - readPart(partsLines); - } - lines.erase(lines.begin(), it); - + assert(lines[0] == "\tParts"); + lines.erase(lines.begin()); + parts->clear(); + auto it = std::find(lines.begin(), lines.end(), "\tKinematicIJs"); + std::vector partsLines(lines.begin(), it); + while (!partsLines.empty()) { + readPart(partsLines); + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readPart(std::vector& lines) { - assert(lines[0] == "\t\tPart"); - lines.erase(lines.begin()); - auto part = CREATE::With(); - part->parseASMT(lines); - parts->push_back(part); - part->owner = this; + assert(lines[0] == "\t\tPart"); + lines.erase(lines.begin()); + auto part = CREATE::With(); + part->parseASMT(lines); + parts->push_back(part); + part->owner = this; } void MbD::ASMTAssembly::readKinematicIJs(std::vector& lines) { - assert(lines[0] == "\tKinematicIJs"); - lines.erase(lines.begin()); - kinematicIJs->clear(); - auto it = std::find(lines.begin(), lines.end(), "\tConstraintSets"); - std::vector kinematicIJsLines(lines.begin(), it); - while (!kinematicIJsLines.empty()) { - readKinematicIJ(kinematicIJsLines); - } - lines.erase(lines.begin(), it); - + assert(lines[0] == "\tKinematicIJs"); + lines.erase(lines.begin()); + kinematicIJs->clear(); + auto it = std::find(lines.begin(), lines.end(), "\tConstraintSets"); + std::vector kinematicIJsLines(lines.begin(), it); + while (!kinematicIJsLines.empty()) { + readKinematicIJ(kinematicIJsLines); + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readKinematicIJ(std::vector&) { - assert(false); + assert(false); } void MbD::ASMTAssembly::readConstraintSets(std::vector& lines) { - assert(lines[0] == "\tConstraintSets"); - lines.erase(lines.begin()); - readJoints(lines); - readMotions(lines); - readLimits(lines); - readGeneralConstraintSets(lines); + assert(lines[0] == "\tConstraintSets"); + lines.erase(lines.begin()); + readJoints(lines); + readMotions(lines); + readLimits(lines); + readGeneralConstraintSets(lines); } void MbD::ASMTAssembly::readJoints(std::vector& lines) { - assert(lines[0] == "\t\tJoints"); - lines.erase(lines.begin()); - joints->clear(); - auto it = std::find(lines.begin(), lines.end(), "\t\tMotions"); - std::vector jointsLines(lines.begin(), it); - std::shared_ptr joint; - while (!jointsLines.empty()) { - if (jointsLines[0] == "\t\t\tAngleJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tGearJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tNoRotationJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tParallelAxesJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tPerpendicularJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tRackPinionJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tScrewJoint") { - joint = CREATE::With(); - } - //AtPointJoints - else if (jointsLines[0] == "\t\t\tConstantVelocityJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tFixedJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tRevoluteJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tSphericalJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tUniversalJoint") { - joint = CREATE::With(); - } - //CompoundJoints - else if (jointsLines[0] == "\t\t\tSphSphJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tCylSphJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tRevCylJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tRevRevJoint") { - joint = CREATE::With(); - } - //InLineJoints - else if (jointsLines[0] == "\t\t\tCylindricalJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tPointInLineJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tTranslationalJoint") { - joint = CREATE::With(); - } - //InPlaneJoints - else if (jointsLines[0] == "\t\t\tLineInPlaneJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tPlanarJoint") { - joint = CREATE::With(); - } - else if (jointsLines[0] == "\t\t\tPointInPlaneJoint") { - joint = CREATE::With(); - } - else { - assert(false); - } - jointsLines.erase(jointsLines.begin()); - joint->parseASMT(jointsLines); - joints->push_back(joint); - joint->owner = this; - } - lines.erase(lines.begin(), it); - + assert(lines[0] == "\t\tJoints"); + lines.erase(lines.begin()); + joints->clear(); + auto it = std::find(lines.begin(), lines.end(), "\t\tMotions"); + std::vector jointsLines(lines.begin(), it); + std::shared_ptr joint; + while (!jointsLines.empty()) { + if (jointsLines[0] == "\t\t\tAngleJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tGearJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tNoRotationJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tParallelAxesJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tPerpendicularJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tRackPinionJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tScrewJoint") { + joint = CREATE::With(); + } + // AtPointJoints + else if (jointsLines[0] == "\t\t\tConstantVelocityJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tFixedJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tRevoluteJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tSphericalJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tUniversalJoint") { + joint = CREATE::With(); + } + // CompoundJoints + else if (jointsLines[0] == "\t\t\tSphSphJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tCylSphJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tRevCylJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tRevRevJoint") { + joint = CREATE::With(); + } + // InLineJoints + else if (jointsLines[0] == "\t\t\tCylindricalJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tPointInLineJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tTranslationalJoint") { + joint = CREATE::With(); + } + // InPlaneJoints + else if (jointsLines[0] == "\t\t\tLineInPlaneJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tPlanarJoint") { + joint = CREATE::With(); + } + else if (jointsLines[0] == "\t\t\tPointInPlaneJoint") { + joint = CREATE::With(); + } + else { + assert(false); + } + jointsLines.erase(jointsLines.begin()); + joint->parseASMT(jointsLines); + joints->push_back(joint); + joint->owner = this; + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readMotions(std::vector& lines) { - assert(lines[0] == "\t\tMotions"); - lines.erase(lines.begin()); - motions->clear(); - auto it = std::find(lines.begin(), lines.end(), "\t\tLimits"); - if (it == lines.end()) { - it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); - } - std::vector motionsLines(lines.begin(), it); - std::shared_ptr motion; - while (!motionsLines.empty()) { - if (motionsLines[0] == "\t\t\tRotationalMotion") { - motion = CREATE::With(); - } - else if (motionsLines[0] == "\t\t\tTranslationalMotion") { - motion = CREATE::With(); - } - else if (motionsLines[0] == "\t\t\tGeneralMotion") { - motion = CREATE::With(); - } - else if (motionsLines[0] == "\t\t\tAllowRotation") { - motion = CREATE::With(); - } - else { - assert(false); - } - motionsLines.erase(motionsLines.begin()); - motion->parseASMT(motionsLines); - motions->push_back(motion); - motion->owner = this; - motion->initMarkers(); - } - lines.erase(lines.begin(), it); - + assert(lines[0] == "\t\tMotions"); + lines.erase(lines.begin()); + motions->clear(); + auto it = std::find(lines.begin(), lines.end(), "\t\tLimits"); + if (it == lines.end()) { + it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); + } + std::vector motionsLines(lines.begin(), it); + std::shared_ptr motion; + while (!motionsLines.empty()) { + if (motionsLines[0] == "\t\t\tRotationalMotion") { + motion = CREATE::With(); + } + else if (motionsLines[0] == "\t\t\tTranslationalMotion") { + motion = CREATE::With(); + } + else if (motionsLines[0] == "\t\t\tGeneralMotion") { + motion = CREATE::With(); + } + else if (motionsLines[0] == "\t\t\tAllowRotation") { + motion = CREATE::With(); + } + else { + assert(false); + } + motionsLines.erase(motionsLines.begin()); + motion->parseASMT(motionsLines); + motions->push_back(motion); + motion->owner = this; + motion->initMarkers(); + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readLimits(std::vector& lines) { - if (lines[0] != "\t\tLimits") return; - lines.erase(lines.begin()); - limits->clear(); - auto it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); - std::vector limitsLines(lines.begin(), it); - std::shared_ptr limit; - while (!limitsLines.empty()) { - if (limitsLines[0] == "\t\t\tRotationLimit") { - limit = ASMTRotationLimit::With(); - } - else if (limitsLines[0] == "\t\t\tTranslationLimit") { - limit = ASMTTranslationLimit::With(); - } - else { - assert(false); - } - limitsLines.erase(limitsLines.begin()); - limit->parseASMT(limitsLines); - limits->push_back(limit); - limit->owner = this; - limit->initMarkers(); - } - lines.erase(lines.begin(), it); + if (lines[0] != "\t\tLimits") { + return; + } + lines.erase(lines.begin()); + limits->clear(); + auto it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); + std::vector limitsLines(lines.begin(), it); + std::shared_ptr limit; + while (!limitsLines.empty()) { + if (limitsLines[0] == "\t\t\tRotationLimit") { + limit = ASMTRotationLimit::With(); + } + else if (limitsLines[0] == "\t\t\tTranslationLimit") { + limit = ASMTTranslationLimit::With(); + } + else { + assert(false); + } + limitsLines.erase(limitsLines.begin()); + limit->parseASMT(limitsLines); + limits->push_back(limit); + limit->owner = this; + limit->initMarkers(); + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readGeneralConstraintSets(std::vector& lines) const { - assert(lines[0] == "\t\tGeneralConstraintSets"); - lines.erase(lines.begin()); - constraintSets->clear(); - auto it = std::find(lines.begin(), lines.end(), "\tForceTorques"); - std::vector generalConstraintSetsLines(lines.begin(), it); - while (!generalConstraintSetsLines.empty()) { - assert(false); - } - lines.erase(lines.begin(), it); + assert(lines[0] == "\t\tGeneralConstraintSets"); + lines.erase(lines.begin()); + constraintSets->clear(); + auto it = std::find(lines.begin(), lines.end(), "\tForceTorques"); + std::vector generalConstraintSetsLines(lines.begin(), it); + while (!generalConstraintSetsLines.empty()) { + assert(false); + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readForcesTorques(std::vector& lines) { - assert(lines[0] == "\tForceTorques"); //Spelling is not consistent in asmt file. - lines.erase(lines.begin()); - forcesTorques->clear(); - auto it = std::find(lines.begin(), lines.end(), "\tConstantGravity"); - std::vector forcesTorquesLines(lines.begin(), it); - while (!forcesTorquesLines.empty()) { - if (forcesTorquesLines[0] == "\t\tForceTorque") { - forcesTorquesLines.erase(forcesTorquesLines.begin()); - auto forceTorque = CREATE::With(); - forceTorque->parseASMT(forcesTorquesLines); - forcesTorques->push_back(forceTorque); - forceTorque->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); + assert(lines[0] == "\tForceTorques"); // Spelling is not consistent in asmt file. + lines.erase(lines.begin()); + forcesTorques->clear(); + auto it = std::find(lines.begin(), lines.end(), "\tConstantGravity"); + std::vector forcesTorquesLines(lines.begin(), it); + while (!forcesTorquesLines.empty()) { + if (forcesTorquesLines[0] == "\t\tForceTorque") { + forcesTorquesLines.erase(forcesTorquesLines.begin()); + auto forceTorque = CREATE::With(); + forceTorque->parseASMT(forcesTorquesLines); + forcesTorques->push_back(forceTorque); + forceTorque->owner = this; + } + else { + assert(false); + } + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readConstantGravity(std::vector& lines) { - assert(lines[0] == "\tConstantGravity"); - lines.erase(lines.begin()); - constantGravity = CREATE::With(); - constantGravity->parseASMT(lines); - constantGravity->owner = this; + assert(lines[0] == "\tConstantGravity"); + lines.erase(lines.begin()); + constantGravity = CREATE::With(); + constantGravity->parseASMT(lines); + constantGravity->owner = this; } void MbD::ASMTAssembly::readSimulationParameters(std::vector& lines) { - assert(lines[0] == "\tSimulationParameters"); - lines.erase(lines.begin()); - simulationParameters = CREATE::With(); - simulationParameters->parseASMT(lines); - simulationParameters->owner = this; + assert(lines[0] == "\tSimulationParameters"); + lines.erase(lines.begin()); + simulationParameters = CREATE::With(); + simulationParameters->parseASMT(lines); + simulationParameters->owner = this; } void MbD::ASMTAssembly::readAnimationParameters(std::vector& lines) { - assert(lines[0] == "\tAnimationParameters"); - lines.erase(lines.begin()); - animationParameters = CREATE::With(); - animationParameters->parseASMT(lines); - animationParameters->owner = this; + assert(lines[0] == "\tAnimationParameters"); + lines.erase(lines.begin()); + animationParameters = CREATE::With(); + animationParameters->parseASMT(lines); + animationParameters->owner = this; } void MbD::ASMTAssembly::readTimeSeries(std::vector& lines) { - if (lines.empty()) return; - assert(lines[0] == "TimeSeries"); - lines.erase(lines.begin()); - assert(lines[0].find("Number\tInput") != std::string::npos); - lines.erase(lines.begin()); - readTimes(lines); + if (lines.empty()) { + return; + } + assert(lines[0] == "TimeSeries"); + lines.erase(lines.begin()); + assert(lines[0].find("Number\tInput") != std::string::npos); + lines.erase(lines.begin()); + readTimes(lines); } void MbD::ASMTAssembly::readTimes(std::vector& lines) { - if (lines.empty()) return; - std::string str = lines[0]; - std::string substr = "Time\tInput"; - auto pos = str.find(substr); - assert(pos != std::string::npos); - str.erase(0, pos + substr.length()); - times = readRowOfDoubles(str); - if (times->empty()) { - times->insert(times->begin(), 0.0); //The first element is the input state. - } - else { - times->insert(times->begin(), times->at(0)); //The first element is the input state. - } - lines.erase(lines.begin()); + if (lines.empty()) { + return; + } + std::string str = lines[0]; + std::string substr = "Time\tInput"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + times = readRowOfDoubles(str); + if (times->empty()) { + times->insert(times->begin(), 0.0); // The first element is the input state. + } + else { + times->insert(times->begin(), times->at(0)); // The first element is the input state. + } + lines.erase(lines.begin()); } void MbD::ASMTAssembly::readPartSeriesMany(std::vector& lines) { - if (lines.empty()) return; - assert(lines[0].find("PartSeries") != std::string::npos); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("JointSeries") != std::string::npos; - }); - std::vector partSeriesLines(lines.begin(), it); - while (!partSeriesLines.empty()) { - readPartSeries(partSeriesLines); - } - lines.erase(lines.begin(), it); + if (lines.empty()) { + return; + } + assert(lines[0].find("PartSeries") != std::string::npos); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("JointSeries") != std::string::npos; + }); + std::vector partSeriesLines(lines.begin(), it); + while (!partSeriesLines.empty()) { + readPartSeries(partSeriesLines); + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readJointSeriesMany(std::vector& lines) { - if (lines.empty()) return; - assert(lines[0].find("JointSeries") != std::string::npos); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("tionSeries") != std::string::npos; - }); - std::vector jointSeriesLines(lines.begin(), it); - while (!jointSeriesLines.empty()) { - readJointSeries(jointSeriesLines); - } - lines.erase(lines.begin(), it); + if (lines.empty()) { + return; + } + assert(lines[0].find("JointSeries") != std::string::npos); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("tionSeries") != std::string::npos; + }); + std::vector jointSeriesLines(lines.begin(), it); + while (!jointSeriesLines.empty()) { + readJointSeries(jointSeriesLines); + } + lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readAssemblySeries(std::vector& lines) { - if (lines.empty()) return; - std::string str = lines[0]; - std::string substr = "AssemblySeries"; - auto pos = str.find(substr); - assert(pos != std::string::npos); - str.erase(0, pos + substr.length()); - auto seriesName = readString(str); - assert(fullName("") == seriesName); - lines.erase(lines.begin()); - //xs, ys, zs, bryxs, bryys, bryzs - readXs(lines); - readYs(lines); - readZs(lines); - readBryantxs(lines); - readBryantys(lines); - readBryantzs(lines); - readVXs(lines); - readVYs(lines); - readVZs(lines); - readOmegaXs(lines); - readOmegaYs(lines); - readOmegaZs(lines); - readAXs(lines); - readAYs(lines); - readAZs(lines); - readAlphaXs(lines); - readAlphaYs(lines); - readAlphaZs(lines); + if (lines.empty()) { + return; + } + std::string str = lines[0]; + std::string substr = "AssemblySeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + assert(fullName("") == seriesName); + lines.erase(lines.begin()); + // xs, ys, zs, bryxs, bryys, bryzs + readXs(lines); + readYs(lines); + readZs(lines); + readBryantxs(lines); + readBryantys(lines); + readBryantzs(lines); + readVXs(lines); + readVYs(lines); + readVZs(lines); + readOmegaXs(lines); + readOmegaYs(lines); + readOmegaZs(lines); + readAXs(lines); + readAYs(lines); + readAZs(lines); + readAlphaXs(lines); + readAlphaYs(lines); + readAlphaZs(lines); } void MbD::ASMTAssembly::readPartSeries(std::vector& lines) { - if (lines.empty()) return; - std::string str = lines[0]; - std::string substr = "PartSeries"; - auto pos = str.find(substr); - assert(pos != std::string::npos); - str.erase(0, pos + substr.length()); - auto seriesName = readString(str); - auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { - return prt->fullName("") == seriesName; - }); - auto& part = *it; - part->readPartSeries(lines); + if (lines.empty()) { + return; + } + std::string str = lines[0]; + std::string substr = "PartSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + return prt->fullName("") == seriesName; + }); + auto& part = *it; + part->readPartSeries(lines); } void MbD::ASMTAssembly::readJointSeries(std::vector& lines) { - if (lines.empty()) return; - std::string str = lines[0]; - std::string substr = "JointSeries"; - auto pos = str.find(substr); - assert(pos != std::string::npos); - str.erase(0, pos + substr.length()); - auto seriesName = readString(str); - auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { - return jt->fullName("") == seriesName; - }); - auto& joint = *it; - joint->readJointSeries(lines); + if (lines.empty()) { + return; + } + std::string str = lines[0]; + std::string substr = "JointSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + auto it = + std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { + return jt->fullName("") == seriesName; + }); + auto& joint = *it; + joint->readJointSeries(lines); } void MbD::ASMTAssembly::readMotionSeriesMany(std::vector& lines) { - while (!lines.empty()) { - assert(lines[0].find("tionSeries") != std::string::npos); - readMotionSeries(lines); - } + while (!lines.empty()) { + assert(lines[0].find("tionSeries") != std::string::npos); + readMotionSeries(lines); + } } void MbD::ASMTAssembly::readMotionSeries(std::vector& lines) { - if (lines.empty()) return; - std::string str = lines[0]; - std::string substr = "tionSeries"; - auto pos = str.find(substr); - assert(pos != std::string::npos); - str.erase(0, pos + substr.length()); - auto seriesName = readString(str); - auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& jt) { - return jt->fullName("") == seriesName; - }); - auto& motion = *it; - motion->readMotionSeries(lines); + if (lines.empty()) { + return; + } + std::string str = lines[0]; + std::string substr = "tionSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + auto it = + std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& jt) { + return jt->fullName("") == seriesName; + }); + auto& motion = *it; + motion->readMotionSeries(lines); } void MbD::ASMTAssembly::runDraggingLog(const char* fileName) { - std::ifstream stream(fileName); - if (stream.fail()) { - throw std::invalid_argument("File not found."); - } - std::string line; - std::vector lines; - while (std::getline(stream, line)) { - lines.push_back(line); - } - assert(readStringOffTop(lines) == "runPreDrag"); - runPreDrag(); - while (lines[0].find("runDragStep") != std::string::npos) - { - assert(readStringOffTop(lines) == "runDragStep"); - auto dragParts = std::make_shared>>(); - while (lines[0].find("Name") != std::string::npos) { - assert(readStringOffTop(lines) == "Name"); - auto dragPartName = readStringOffTop(lines); - std::string longerName = "/" + name + "/" + dragPartName; - auto dragPart = partAt(longerName); - dragParts->push_back(dragPart); - assert(readStringOffTop(lines) == "Position3D"); - auto dragPartPosition3D = readColumnOfDoublesOffTop(lines); - dragPart->updateMbDFromPosition3D(dragPartPosition3D); - assert(readStringOffTop(lines) == "RotationMatrix"); - auto dragPartRotationMatrix = std::make_shared>(3); - for (size_t i = 0; i < 3; i++) - { - auto row = readRowOfDoublesOffTop(lines); - dragPartRotationMatrix->atiput(i, row); - } - dragPart->updateMbDFromRotationMatrix(dragPartRotationMatrix); - } - runDragStep(dragParts); - } - assert(readStringOffTop(lines) == "runPostDrag"); - runPostDrag(); + std::ifstream stream(fileName); + if (stream.fail()) { + throw std::invalid_argument("File not found."); + } + std::string line; + std::vector lines; + while (std::getline(stream, line)) { + lines.push_back(line); + } + assert(readStringOffTop(lines) == "runPreDrag"); + runPreDrag(); + while (lines[0].find("runDragStep") != std::string::npos) { + assert(readStringOffTop(lines) == "runDragStep"); + auto dragParts = std::make_shared>>(); + while (lines[0].find("Name") != std::string::npos) { + assert(readStringOffTop(lines) == "Name"); + auto dragPartName = readStringOffTop(lines); + std::string longerName = "/" + name + "/" + dragPartName; + auto dragPart = partAt(longerName); + dragParts->push_back(dragPart); + assert(readStringOffTop(lines) == "Position3D"); + auto dragPartPosition3D = readColumnOfDoublesOffTop(lines); + dragPart->setPosition3D(dragPartPosition3D); + assert(readStringOffTop(lines) == "RotationMatrix"); + auto dragPartRotationMatrix = std::make_shared>(3); + for (size_t i = 0; i < 3; i++) { + auto row = readRowOfDoublesOffTop(lines); + dragPartRotationMatrix->atiput(i, row); + } + dragPart->setRotationMatrix(dragPartRotationMatrix); + } + runDragStep(dragParts); + } + assert(readStringOffTop(lines) == "runPostDrag"); + runPostDrag(); } void MbD::ASMTAssembly::outputFor(AnalysisType) { - assert(false); + assert(false); } void MbD::ASMTAssembly::preMbDrun(std::shared_ptr mbdSys) { - calcCharacteristicDimensions(); - deleteMbD(); - createMbD(mbdSys, mbdUnits); - std::static_pointer_cast(mbdObject)->asFixed(); + calcCharacteristicDimensions(); + deleteMbD(); + createMbD(mbdSys, mbdUnits); + std::static_pointer_cast(mbdObject)->asFixed(); +} + +void MbD::ASMTAssembly::preMbDrunDragStep(std::shared_ptr mbdSys, std::shared_ptr>> dragParts) +{ + for (auto& part : *parts) { + part->preMbDrunDragStep(mbdSys, mbdUnits); + } } void MbD::ASMTAssembly::postMbDrun() { - assert(false); + assert(false); } void MbD::ASMTAssembly::calcCharacteristicDimensions() { - auto unitTime = this->calcCharacteristicTime(); - auto unitMass = this->calcCharacteristicMass(); - auto unitLength = this->calcCharacteristicLength(); - auto unitAngle = 1.0; - this->mbdUnits = std::make_shared(unitTime, unitMass, unitLength, unitAngle); - this->mbdUnits = std::make_shared(1.0, 1.0, 1.0, 1.0); //for debug + auto unitTime = this->calcCharacteristicTime(); + auto unitMass = this->calcCharacteristicMass(); + auto unitLength = this->calcCharacteristicLength(); + auto unitAngle = 1.0; + this->mbdUnits = std::make_shared(unitTime, unitMass, unitLength, unitAngle); + this->mbdUnits = std::make_shared(1.0, 1.0, 1.0, 1.0); // for debug } double MbD::ASMTAssembly::calcCharacteristicTime() const { - return std::abs(simulationParameters->hout); + return std::abs(simulationParameters->hout); } double MbD::ASMTAssembly::calcCharacteristicMass() const { - auto n = parts->size(); - double sumOfSquares = 0.0; - for (size_t i = 0; i < n; i++) - { - auto mass = parts->at(i)->principalMassMarker->mass; - sumOfSquares += mass * mass; - } - auto unitMass = std::sqrt(sumOfSquares / n); - if (unitMass <= 0) unitMass = 1.0; - return unitMass; + auto n = parts->size(); + double sumOfSquares = 0.0; + for (size_t i = 0; i < n; i++) { + auto mass = parts->at(i)->principalMassMarker->mass; + sumOfSquares += mass * mass; + } + auto unitMass = std::sqrt(sumOfSquares / n); + if (unitMass <= 0) { + unitMass = 1.0; + } + return unitMass; } double MbD::ASMTAssembly::calcCharacteristicLength() const { - auto markerMap = this->markerMap(); - auto lengths = std::make_shared>(); - auto connectorList = this->connectorList(); - for (auto& connector : *connectorList) { - auto& mkrI = markerMap->at(connector->markerI); - lengths->push_back(mkrI->rpmp()->length()); - auto& mkrJ = markerMap->at(connector->markerJ); - lengths->push_back(mkrJ->rpmp()->length()); - } - auto n = lengths->size(); - double sumOfSquares = std::accumulate(lengths->begin(), lengths->end(), 0.0, [](double sum, double l) { return sum + l * l; }); - auto unitLength = std::sqrt(sumOfSquares / std::max(n, size_t(1))); - if (unitLength <= 0) unitLength = 1.0; - return unitLength; + auto markerMap = this->markerMap(); + auto lengths = std::make_shared>(); + auto connectorList = this->connectorList(); + for (auto& connector : *connectorList) { + auto& mkrI = markerMap->at(connector->markerI); + lengths->push_back(mkrI->rpmp()->length()); + auto& mkrJ = markerMap->at(connector->markerJ); + lengths->push_back(mkrJ->rpmp()->length()); + } + auto n = lengths->size(); + double sumOfSquares = + std::accumulate(lengths->begin(), lengths->end(), 0.0, [](double sum, double l) { + return sum + l * l; + }); + auto unitLength = std::sqrt(sumOfSquares / std::max(n, size_t(1))); + if (unitLength <= 0) { + unitLength = 1.0; + } + return unitLength; } std::shared_ptr>> MbD::ASMTAssembly::connectorList() const { - auto list = std::make_shared>>(); - list->insert(list->end(), joints->begin(), joints->end()); - list->insert(list->end(), motions->begin(), motions->end()); - list->insert(list->end(), kinematicIJs->begin(), kinematicIJs->end()); - list->insert(list->end(), forcesTorques->begin(), forcesTorques->end()); - return list; -} - -std::shared_ptr>> MbD::ASMTAssembly::markerMap() const -{ - auto answer = std::make_shared>>(); - for (auto& refPoint : *refPoints) { - for (auto& marker : *refPoint->markers) { - answer->insert(std::make_pair(marker->fullName(""), marker)); - } - } - for (auto& part : *parts) { - for (auto& refPoint : *part->refPoints) { - for (auto& marker : *refPoint->markers) { - answer->insert(std::make_pair(marker->fullName(""), marker)); - } - } - } - return answer; + auto list = std::make_shared>>(); + list->insert(list->end(), joints->begin(), joints->end()); + list->insert(list->end(), motions->begin(), motions->end()); + list->insert(list->end(), kinematicIJs->begin(), kinematicIJs->end()); + list->insert(list->end(), forcesTorques->begin(), forcesTorques->end()); + return list; +} + +std::shared_ptr>> +MbD::ASMTAssembly::markerMap() const +{ + auto answer = std::make_shared>>(); + for (auto& refPoint : *refPoints) { + for (auto& marker : *refPoint->markers) { + answer->insert(std::make_pair(marker->fullName(""), marker)); + } + } + for (auto& part : *parts) { + for (auto& refPoint : *part->refPoints) { + for (auto& marker : *refPoint->markers) { + answer->insert(std::make_pair(marker->fullName(""), marker)); + } + } + } + return answer; } void MbD::ASMTAssembly::deleteMbD() { - ASMTSpatialContainer::deleteMbD(); - constantGravity->deleteMbD(); - asmtTime->deleteMbD(); - for (auto& part : *parts) { part->deleteMbD(); } - for (auto& joint : *joints) { joint->deleteMbD(); } - for (auto& motion : *motions) { motion->deleteMbD(); } - for (auto& limit : *limits) { limit->deleteMbD(); } - for (auto& forceTorque : *forcesTorques) { forceTorque->deleteMbD(); } - - + ASMTSpatialContainer::deleteMbD(); + constantGravity->deleteMbD(); + asmtTime->deleteMbD(); + for (auto& part : *parts) { + part->deleteMbD(); + } + for (auto& joint : *joints) { + joint->deleteMbD(); + } + for (auto& motion : *motions) { + motion->deleteMbD(); + } + for (auto& limit : *limits) { + limit->deleteMbD(); + } + for (auto& forceTorque : *forcesTorques) { + forceTorque->deleteMbD(); + } } void MbD::ASMTAssembly::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) { - ASMTSpatialContainer::createMbD(mbdSys, mbdUnits); - constantGravity->createMbD(mbdSys, mbdUnits); - asmtTime->createMbD(mbdSys, mbdUnits); - std::sort(parts->begin(), parts->end(), [](std::shared_ptr a, std::shared_ptr b) { return a->name < b->name; }); - auto jointsMotions = std::make_shared>>(); - jointsMotions->insert(jointsMotions->end(), joints->begin(), joints->end()); - jointsMotions->insert(jointsMotions->end(), motions->begin(), motions->end()); - std::sort(jointsMotions->begin(), jointsMotions->end(), [](std::shared_ptr a, std::shared_ptr b) { return a->name < b->name; }); - std::sort(limits->begin(), limits->end(), [](std::shared_ptr a, std::shared_ptr b) { return a->name < b->name; }); - std::sort(forcesTorques->begin(), forcesTorques->end(), [](std::shared_ptr a, std::shared_ptr b) { return a->name < b->name; }); - for (auto& part : *parts) { part->createMbD(mbdSys, mbdUnits); } - for (auto& joint : *jointsMotions) { joint->createMbD(mbdSys, mbdUnits); } - for (auto& limit : *limits) { limit->createMbD(mbdSys, mbdUnits); } - for (auto& forceTorque : *forcesTorques) { forceTorque->createMbD(mbdSys, mbdUnits); } - - auto& mbdSysSolver = mbdSys->systemSolver; - mbdSysSolver->errorTolPosKine = simulationParameters->errorTolPosKine; - mbdSysSolver->errorTolAccKine = simulationParameters->errorTolAccKine; - mbdSysSolver->iterMaxPosKine = simulationParameters->iterMaxPosKine; - mbdSysSolver->iterMaxAccKine = simulationParameters->iterMaxAccKine; - mbdSysSolver->tstart = simulationParameters->tstart / mbdUnits->time; - mbdSysSolver->tend = simulationParameters->tend / mbdUnits->time; - mbdSysSolver->hmin = simulationParameters->hmin / mbdUnits->time; - mbdSysSolver->hmax = simulationParameters->hmax / mbdUnits->time; - mbdSysSolver->hout = simulationParameters->hout / mbdUnits->time; - mbdSysSolver->corAbsTol = simulationParameters->corAbsTol; - mbdSysSolver->corRelTol = simulationParameters->corRelTol; - mbdSysSolver->intAbsTol = simulationParameters->intAbsTol; - mbdSysSolver->intRelTol = simulationParameters->intRelTol; - mbdSysSolver->iterMaxDyn = simulationParameters->iterMaxDyn; - mbdSysSolver->orderMax = simulationParameters->orderMax; - mbdSysSolver->translationLimit = simulationParameters->translationLimit / mbdUnits->length; - mbdSysSolver->rotationLimit = simulationParameters->rotationLimit; - //animationParameters = nullptr; + ASMTSpatialContainer::createMbD(mbdSys, mbdUnits); + constantGravity->createMbD(mbdSys, mbdUnits); + asmtTime->createMbD(mbdSys, mbdUnits); + std::sort(parts->begin(), + parts->end(), + [](std::shared_ptr a, std::shared_ptr b) { + return a->name < b->name; + }); + auto jointsMotions = std::make_shared>>(); + jointsMotions->insert(jointsMotions->end(), joints->begin(), joints->end()); + jointsMotions->insert(jointsMotions->end(), motions->begin(), motions->end()); + std::sort(jointsMotions->begin(), + jointsMotions->end(), + [](std::shared_ptr a, std::shared_ptr b) { + return a->name < b->name; + }); + std::sort(limits->begin(), + limits->end(), + [](std::shared_ptr a, std::shared_ptr b) { + return a->name < b->name; + }); + std::sort(forcesTorques->begin(), + forcesTorques->end(), + [](std::shared_ptr a, std::shared_ptr b) { + return a->name < b->name; + }); + for (auto& part : *parts) { + part->createMbD(mbdSys, mbdUnits); + } + for (auto& joint : *jointsMotions) { + joint->createMbD(mbdSys, mbdUnits); + } + for (auto& limit : *limits) { + limit->createMbD(mbdSys, mbdUnits); + } + for (auto& forceTorque : *forcesTorques) { + forceTorque->createMbD(mbdSys, mbdUnits); + } + + auto& mbdSysSolver = mbdSys->systemSolver; + mbdSysSolver->errorTolPosKine = simulationParameters->errorTolPosKine; + mbdSysSolver->errorTolAccKine = simulationParameters->errorTolAccKine; + mbdSysSolver->iterMaxPosKine = simulationParameters->iterMaxPosKine; + mbdSysSolver->iterMaxAccKine = simulationParameters->iterMaxAccKine; + mbdSysSolver->tstart = simulationParameters->tstart / mbdUnits->time; + mbdSysSolver->tend = simulationParameters->tend / mbdUnits->time; + mbdSysSolver->hmin = simulationParameters->hmin / mbdUnits->time; + mbdSysSolver->hmax = simulationParameters->hmax / mbdUnits->time; + mbdSysSolver->hout = simulationParameters->hout / mbdUnits->time; + mbdSysSolver->corAbsTol = simulationParameters->corAbsTol; + mbdSysSolver->corRelTol = simulationParameters->corRelTol; + mbdSysSolver->intAbsTol = simulationParameters->intAbsTol; + mbdSysSolver->intRelTol = simulationParameters->intRelTol; + mbdSysSolver->iterMaxDyn = simulationParameters->iterMaxDyn; + mbdSysSolver->orderMax = simulationParameters->orderMax; + mbdSysSolver->translationLimit = simulationParameters->translationLimit / mbdUnits->length; + mbdSysSolver->rotationLimit = simulationParameters->rotationLimit; + // animationParameters = nullptr; } void MbD::ASMTAssembly::outputFile(std::string filename) { - std::ofstream os(filename); - os << std::setprecision(std::numeric_limits::max_digits10); - // try { - os << "OndselSolver" << std::endl; - storeOnLevel(os, 0); - os.close(); - // } - // catch (...) { - // os.close(); - // } + std::ofstream os(filename); + os << std::setprecision(std::numeric_limits::max_digits10); + // try { + os << "OndselSolver" << std::endl; + storeOnLevel(os, 0); + os.close(); + // } + // catch (...) { + // os.close(); + // } } void MbD::ASMTAssembly::storeOnLevel(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "Assembly"); - storeOnLevelNotes(os, level + 1); - storeOnLevelName(os, level + 1); - ASMTSpatialContainer::storeOnLevel(os, level); + storeOnLevelString(os, level, "Assembly"); + storeOnLevelNotes(os, level + 1); + storeOnLevelName(os, level + 1); + ASMTSpatialContainer::storeOnLevel(os, level); - storeOnLevelParts(os, level + 1); - storeOnLevelKinematicIJs(os, level + 1); - storeOnLevelConstraintSets(os, level + 1); - storeOnLevelForceTorques(os, level + 1); - constantGravity->storeOnLevel(os, level + 1); - simulationParameters->storeOnLevel(os, level + 1); - animationParameters->storeOnLevel(os, level + 1); - storeOnTimeSeries(os); + storeOnLevelParts(os, level + 1); + storeOnLevelKinematicIJs(os, level + 1); + storeOnLevelConstraintSets(os, level + 1); + storeOnLevelForceTorques(os, level + 1); + constantGravity->storeOnLevel(os, level + 1); + simulationParameters->storeOnLevel(os, level + 1); + animationParameters->storeOnLevel(os, level + 1); + storeOnTimeSeries(os); } void MbD::ASMTAssembly::solve() { - auto simulationParameters = CREATE::With(); - simulationParameters->settstart(0.0); - simulationParameters->settend(0.0); //tstart == tend Initial Conditions only. - simulationParameters->sethmin(1.0e-9); - simulationParameters->sethmax(1.0); - simulationParameters->sethout(0.04); - simulationParameters->seterrorTol(1.0e-6); - setSimulationParameters(simulationParameters); + auto simulationParameters = CREATE::With(); + simulationParameters->settstart(0.0); + simulationParameters->settend(0.0); // tstart == tend Initial Conditions only. + simulationParameters->sethmin(1.0e-9); + simulationParameters->sethmax(1.0); + simulationParameters->sethout(0.04); + simulationParameters->seterrorTol(1.0e-6); + setSimulationParameters(simulationParameters); - runKINEMATIC(); + runKINEMATIC(); } void MbD::ASMTAssembly::runPreDrag() { - if (debug) { - outputFile("runPreDrag.asmt"); - std::ofstream os("dragging.log"); - os << "runPreDrag" << std::endl; - os.close(); - } - mbdSystem = std::make_shared(); - mbdSystem->externalSystem->asmtAssembly = this; - mbdSystem->runPreDrag(mbdSystem); -} - -void MbD::ASMTAssembly::runDragStep(std::shared_ptr>> dragParts) -{ - if (debug) { - std::ofstream os("dragging.log", std::ios_base::app); - os << "runDragStep" << std::endl; - os.close(); - } - auto dragMbDParts = std::make_shared>>(); - for (auto& dragPart : *dragParts) { - if (debug) { - std::ofstream os("dragging.log", std::ios_base::app); - os << std::setprecision(std::numeric_limits::max_digits10); - dragPart->storeOnLevelName(os, 1); - dragPart->storeOnLevelPositionRaw(os, 1); - dragPart->storeOnLevelRotationMatrixRaw(os, 1); - os.close(); - } - auto dragMbDPart = std::static_pointer_cast(dragPart->mbdObject); - dragMbDParts->push_back(dragMbDPart); - } - try { - mbdSystem->runDragStep(dragMbDParts); - } - catch (...) { - //Do not use - //runPreDrag(); - //Assembly breaks up too easily because of redundant constraint removal. - noop(); - } + if (debug) { + outputFile("runPreDrag.asmt"); + std::ofstream os("dragging.log"); + os << "runPreDrag" << std::endl; + os.close(); + } + mbdSystem = std::make_shared(); + mbdSystem->externalSystem->asmtAssembly = this; + mbdSystem->runPreDrag(mbdSystem); +} + +void MbD::ASMTAssembly::runDragStep( + std::shared_ptr>> dragASMTParts) +{ + if (debug) { + std::ofstream os("dragging.log", std::ios_base::app); + os << "runDragStep" << std::endl; + os.close(); + } + auto dragMbDParts = std::make_shared>>(); + auto crO1 = std::make_shared>(); + auto crO2 = std::make_shared>(); + auto cqEO1 = std::make_shared>>>(); + auto cqEO2 = std::make_shared>>>(); + for (auto& dragASMTPart : *dragASMTParts) { + if (debug) { + std::ofstream os("dragging.log", std::ios_base::app); + os << std::setprecision(std::numeric_limits::max_digits10); + dragASMTPart->storeOnLevelName(os, 1); + dragASMTPart->storeOnLevelPositionRaw(os, 1); + dragASMTPart->storeOnLevelRotationMatrixRaw(os, 1); + os.close(); + } + auto dragMbDPart = std::static_pointer_cast(dragASMTPart->mbdObject); + dragMbDParts->push_back(dragMbDPart); + crO1->push_back(dragASMTPart->oldPos3D); + crO2->push_back(dragASMTPart->position3D); + cqEO1->push_back(dragASMTPart->oldRotMat->asEulerParameters()); + cqEO2->push_back(dragASMTPart->rotationMatrix->asEulerParameters()); + } + bool success = false; + for (int i = 0; i < 5; i++) { + if (i > 0) { + double factor = std::pow(2.0, -i); + for (size_t j = 0; j < dragASMTParts->size(); j++) { + auto& dragASMTPart = dragASMTParts->at(j); + auto rO1 = crO1->at(j); + auto rO2 = crO2->at(j); + auto rOMid = rO1->times(1.0 - factor)->plusFullColumn(rO2->times(factor)); + dragASMTPart->setPosition3D(rOMid); + auto qEO1 = cqEO1->at(j); + auto qEO2 = cqEO2->at(j); + std::shared_ptr> qEOmid; + auto cosHalfTheta = qEO1->dot(qEO2); + if (abs(cosHalfTheta) >= 1.0) { + qEOmid = qEO1->copy(); + } + else { + auto halfTheta = std::acos(cosHalfTheta); + auto sinHalfTheta = std::sin(halfTheta); + double ratio1 = std::sin((1.0 - factor) * halfTheta) / sinHalfTheta; + double ratio2 = std::sin(factor * halfTheta) / sinHalfTheta; + qEOmid = qEO1->times(ratio1)->plusFullColumn(qEO2->times(ratio2)); + } + qEOmid->calcABC(); + dragASMTPart->setRotationMatrix(qEOmid->aA); + } + } + if (debug) { + outputFile("runDragStep.asmt"); + } + try { + mbdSystem->runDragStep(mbdSystem, dragMbDParts); + success = true; + break; + } + catch (std::exception const& e) { + // Do not use + // runPreDrag(); + // Assembly breaks up too easily because of redundant constraint removal. + noop(); + } + } + if (!success) restorePosRot(); } void MbD::ASMTAssembly::runPostDrag() { - if (debug) { - //outputFile("runPostDrag.asmt"); - std::ofstream os("dragging.log", std::ios_base::app); - os << "runPostDrag" << std::endl; - os.close(); - } - mbdSystem = std::make_shared(); - mbdSystem->externalSystem->asmtAssembly = this; - mbdSystem->runPreDrag(mbdSystem); + if (debug) { + // outputFile("runPostDrag.asmt"); + std::ofstream os("dragging.log", std::ios_base::app); + os << "runPostDrag" << std::endl; + os.close(); + } + mbdSystem = std::make_shared(); + mbdSystem->externalSystem->asmtAssembly = this; + mbdSystem->runPreDrag(mbdSystem); } -void MbD::ASMTAssembly::runKINEMATIC() +void MbD::ASMTAssembly::restorePosRot() { - mbdSystem = std::make_shared(); - mbdSystem->externalSystem->asmtAssembly = this; - try { - mbdSystem->runKINEMATIC(mbdSystem); - } - catch (SimulationStoppingError ex) { + for (auto& part : *parts) { + part->restorePosRot(); + } +} - } +void MbD::ASMTAssembly::runKINEMATIC() +{ + mbdSystem = std::make_shared(); + mbdSystem->externalSystem->asmtAssembly = this; + try { + mbdSystem->runKINEMATIC(mbdSystem); + } + catch (SimulationStoppingError ex) { + } } void MbD::ASMTAssembly::initprincipalMassMarker() { - principalMassMarker = ASMTPrincipalMassMarker::With(); - principalMassMarker->mass = 0.0; - principalMassMarker->density = 0.0; - principalMassMarker->momentOfInertias = std::make_shared>(3, 0); - //principalMassMarker->position3D = std::make_shared>(3, 0); - //principalMassMarker->rotationMatrix = FullMatrix>::identitysptr(3); + principalMassMarker = ASMTPrincipalMassMarker::With(); + principalMassMarker->mass = 0.0; + principalMassMarker->density = 0.0; + principalMassMarker->momentOfInertias = std::make_shared>(3, 0); + // principalMassMarker->position3D = std::make_shared>(3, 0); + // principalMassMarker->rotationMatrix = FullMatrix>::identitysptr(3); } -std::shared_ptr MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr self, std::string& longname) const +std::shared_ptr +MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr self, + std::string& longname) const { - if ((self->fullName("")) == longname) return self; - auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { - return prt->fullName("") == longname; - }); - auto& part = *it; - return part; + if ((self->fullName("")) == longname) { + return self; + } + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + return prt->fullName("") == longname; + }); + auto& part = *it; + return part; } std::shared_ptr MbD::ASMTAssembly::partAt(std::string& longname) const { - for (auto& part : *parts) { - if (part->fullName("") == longname) return part; - } - return nullptr; + for (auto& part : *parts) { + if (part->fullName("") == longname) { + return part; + } + } + return nullptr; } std::shared_ptr MbD::ASMTAssembly::markerAt(std::string& longname) const { - for (auto& refPoint : *refPoints) { - for (auto& marker : *refPoint->markers) { - if (marker->fullName("") == longname) return marker; - } - } - for (auto& part : *parts) { - for (auto& refPoint : *part->refPoints) { - for (auto& marker : *refPoint->markers) { - if (marker->fullName("") == longname) return marker; - } - } - } - return nullptr; + for (auto& refPoint : *refPoints) { + for (auto& marker : *refPoint->markers) { + if (marker->fullName("") == longname) { + return marker; + } + } + } + for (auto& part : *parts) { + for (auto& refPoint : *part->refPoints) { + for (auto& marker : *refPoint->markers) { + if (marker->fullName("") == longname) { + return marker; + } + } + } + } + return nullptr; } std::shared_ptr MbD::ASMTAssembly::jointAt(std::string& longname) const { - auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { - return jt->fullName("") == longname; - }); - auto& joint = *it; - return joint; + auto it = + std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { + return jt->fullName("") == longname; + }); + auto& joint = *it; + return joint; } std::shared_ptr MbD::ASMTAssembly::motionAt(std::string& longname) const { - auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& mt) { - return mt->fullName("") == longname; - }); - auto& motion = *it; - return motion; + auto it = + std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& mt) { + return mt->fullName("") == longname; + }); + auto& motion = *it; + return motion; } std::shared_ptr MbD::ASMTAssembly::forceTorqueAt(std::string& longname) const { - auto it = std::find_if(forcesTorques->begin(), forcesTorques->end(), [&](const std::shared_ptr& mt) { - return mt->fullName("") == longname; - }); - auto& forceTorque = *it; - return forceTorque; + auto it = std::find_if(forcesTorques->begin(), + forcesTorques->end(), + [&](const std::shared_ptr& mt) { + return mt->fullName("") == longname; + }); + auto& forceTorque = *it; + return forceTorque; } FColDsptr MbD::ASMTAssembly::vOcmO() { - return std::make_shared>(3, 0.0); + return std::make_shared>(3, 0.0); } FColDsptr MbD::ASMTAssembly::omeOpO() { - return std::make_shared>(3, 0.0); + return std::make_shared>(3, 0.0); } std::shared_ptr MbD::ASMTAssembly::geoTime() const { - return asmtTime; + return asmtTime; } void MbD::ASMTAssembly::updateFromMbD() { - ASMTSpatialContainer::updateFromMbD(); - auto time = asmtTime->getValue(); - times->push_back(time); - std::cout << "Time = " << time << std::endl; - for (auto& part : *parts) part->updateFromMbD(); - for (auto& joint : *joints) joint->updateFromMbD(); - for (auto& motion : *motions) motion->updateFromMbD(); - for (auto& forceTorque : *forcesTorques) forceTorque->updateFromMbD(); + ASMTSpatialContainer::updateFromMbD(); + auto time = asmtTime->getValue(); + times->push_back(time); + std::cout << "Time = " << time << std::endl; + for (auto& part : *parts) { + part->updateFromMbD(); + } + for (auto& joint : *joints) { + joint->updateFromMbD(); + } + for (auto& motion : *motions) { + motion->updateFromMbD(); + } + for (auto& forceTorque : *forcesTorques) { + forceTorque->updateFromMbD(); + } } void MbD::ASMTAssembly::compareResults(AnalysisType type) { - ASMTSpatialContainer::compareResults(type); - for (auto& part : *parts) part->compareResults(type); - for (auto& joint : *joints) joint->compareResults(type); - for (auto& motion : *motions) motion->compareResults(type); - for (auto& forceTorque : *forcesTorques) forceTorque->compareResults(type); + ASMTSpatialContainer::compareResults(type); + for (auto& part : *parts) { + part->compareResults(type); + } + for (auto& joint : *joints) { + joint->compareResults(type); + } + for (auto& motion : *motions) { + motion->compareResults(type); + } + for (auto& forceTorque : *forcesTorques) { + forceTorque->compareResults(type); + } } void MbD::ASMTAssembly::outputResults(AnalysisType type) @@ -1484,167 +1584,172 @@ void MbD::ASMTAssembly::outputResults(AnalysisType type) void MbD::ASMTAssembly::addPart(std::shared_ptr part) { - parts->push_back(part); - part->owner = this; + parts->push_back(part); + part->owner = this; } void MbD::ASMTAssembly::addJoint(std::shared_ptr joint) { - joints->push_back(joint); - joint->owner = this; + joints->push_back(joint); + joint->owner = this; } void MbD::ASMTAssembly::addMotion(std::shared_ptr motion) { - motions->push_back(motion); - motion->owner = this; - motion->initMarkers(); + motions->push_back(motion); + motion->owner = this; + motion->initMarkers(); } void MbD::ASMTAssembly::addLimit(std::shared_ptr limit) { - limits->push_back(limit); - limit->owner = this; - limit->initMarkers(); + limits->push_back(limit); + limit->owner = this; + limit->initMarkers(); } void MbD::ASMTAssembly::setConstantGravity(std::shared_ptr gravity) { - constantGravity = gravity; - gravity->owner = this; + constantGravity = gravity; + gravity->owner = this; } -void MbD::ASMTAssembly::setSimulationParameters(std::shared_ptr parameters) +void MbD::ASMTAssembly::setSimulationParameters( + std::shared_ptr parameters) { - simulationParameters = parameters; - parameters->owner = this; + simulationParameters = parameters; + parameters->owner = this; } std::shared_ptr MbD::ASMTAssembly::partNamed(std::string partName) const { - auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { - return prt->fullName("") == partName; - }); - auto& part = *it; - return part; + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + return prt->fullName("") == partName; + }); + auto& part = *it; + return part; } std::shared_ptr MbD::ASMTAssembly::partPartialNamed(std::string partialName) const { - auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { - auto fullName = prt->fullName(""); - return fullName.find(partialName) != std::string::npos; - }); - auto& part = *it; - return part; + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + auto fullName = prt->fullName(""); + return fullName.find(partialName) != std::string::npos; + }); + auto& part = *it; + return part; } void MbD::ASMTAssembly::storeOnLevelNotes(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "Notes"); - storeOnLevelString(os, level + 1, notes); + storeOnLevelString(os, level, "Notes"); + storeOnLevelString(os, level + 1, notes); } void MbD::ASMTAssembly::storeOnLevelParts(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "Parts"); - for (auto& part : *parts) { - part->storeOnLevel(os, level + 1); - } + storeOnLevelString(os, level, "Parts"); + for (auto& part : *parts) { + part->storeOnLevel(os, level + 1); + } } void MbD::ASMTAssembly::storeOnLevelKinematicIJs(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "KinematicIJs"); - for (auto& kinematicIJ : *kinematicIJs) { - kinematicIJ->storeOnLevel(os, level); - } + storeOnLevelString(os, level, "KinematicIJs"); + for (auto& kinematicIJ : *kinematicIJs) { + kinematicIJ->storeOnLevel(os, level); + } } void MbD::ASMTAssembly::storeOnLevelConstraintSets(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "ConstraintSets"); - storeOnLevelJoints(os, level + 1); - storeOnLevelMotions(os, level + 1); - storeOnLevelLimits(os, level + 1); - storeOnLevelGeneralConstraintSets(os, level + 1); + storeOnLevelString(os, level, "ConstraintSets"); + storeOnLevelJoints(os, level + 1); + storeOnLevelMotions(os, level + 1); + storeOnLevelLimits(os, level + 1); + storeOnLevelGeneralConstraintSets(os, level + 1); } void MbD::ASMTAssembly::storeOnLevelForceTorques(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "ForceTorques"); - for (auto& forceTorque : *forcesTorques) { - forceTorque->storeOnLevel(os, level + 1); - } + storeOnLevelString(os, level, "ForceTorques"); + for (auto& forceTorque : *forcesTorques) { + forceTorque->storeOnLevel(os, level + 1); + } } void MbD::ASMTAssembly::storeOnLevelJoints(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "Joints"); - for (auto& joint : *joints) { - joint->storeOnLevel(os, level + 1); - } + storeOnLevelString(os, level, "Joints"); + for (auto& joint : *joints) { + joint->storeOnLevel(os, level + 1); + } } void MbD::ASMTAssembly::storeOnLevelMotions(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "Motions"); - for (auto& motion : *motions) { - motion->storeOnLevel(os, level + 1); - } + storeOnLevelString(os, level, "Motions"); + for (auto& motion : *motions) { + motion->storeOnLevel(os, level + 1); + } } void MbD::ASMTAssembly::storeOnLevelLimits(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "Limits"); - for (auto& limit : *limits) { - limit->storeOnLevel(os, level + 1); - } + storeOnLevelString(os, level, "Limits"); + for (auto& limit : *limits) { + limit->storeOnLevel(os, level + 1); + } } void MbD::ASMTAssembly::storeOnLevelGeneralConstraintSets(std::ofstream& os, size_t level) { - storeOnLevelString(os, level, "GeneralConstraintSets"); - //for (auto& generalConstraintSet : *generalConstraintSets) { - // generalConstraintSet->storeOnLevel(os, level); - //} + storeOnLevelString(os, level, "GeneralConstraintSets"); + // for (auto& generalConstraintSet : *generalConstraintSets) { + // generalConstraintSet->storeOnLevel(os, level); + // } } void MbD::ASMTAssembly::storeOnTimeSeries(std::ofstream& os) { - if (times->empty()) return; - os << "TimeSeries" << std::endl; - os << "Number\tInput\t"; - for (size_t i = 1; i < times->size(); i++) - { - os << i << '\t'; - } - os << std::endl; - os << "Time\tInput\t"; - for (size_t i = 1; i < times->size(); i++) - { - os << times->at(i) << '\t'; - } - os << std::endl; - os << "AssemblySeries\t" << fullName("") << std::endl; - ASMTSpatialContainer::storeOnTimeSeries(os); - for (auto& part : *parts) part->storeOnTimeSeries(os); - for (auto& joint : *joints) joint->storeOnTimeSeries(os); - for (auto& motion : *motions) motion->storeOnTimeSeries(os); + if (times->empty()) { + return; + } + os << "TimeSeries" << std::endl; + os << "Number\tInput\t"; + for (size_t i = 1; i < times->size(); i++) { + os << i << '\t'; + } + os << std::endl; + os << "Time\tInput\t"; + for (size_t i = 1; i < times->size(); i++) { + os << times->at(i) << '\t'; + } + os << std::endl; + os << "AssemblySeries\t" << fullName("") << std::endl; + ASMTSpatialContainer::storeOnTimeSeries(os); + for (auto& part : *parts) { + part->storeOnTimeSeries(os); + } + for (auto& joint : *joints) { + joint->storeOnTimeSeries(os); + } + for (auto& motion : *motions) { + motion->storeOnTimeSeries(os); + } } void MbD::ASMTAssembly::setFilename(std::string str) { - std::stringstream ss; - ss << "FileName = " << str << std::endl; - auto str2 = ss.str(); - logString(str2); - filename = str; + std::stringstream ss; + ss << "FileName = " << str << std::endl; + auto str2 = ss.str(); + logString(str2); + filename = str; } void MbD::ASMTAssembly::setDebug(bool todebug) { - debug = todebug; + debug = todebug; } - - diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h index 81b6ce7..d50172a 100644 --- a/OndselSolver/ASMTAssembly.h +++ b/OndselSolver/ASMTAssembly.h @@ -79,6 +79,7 @@ namespace MbD { void outputFor(AnalysisType type); void preMbDrun(std::shared_ptr mbdSys); + void preMbDrunDragStep(std::shared_ptr mbdSys, std::shared_ptr>> dragParts); void postMbDrun(); void calcCharacteristicDimensions(); double calcCharacteristicTime() const; @@ -97,6 +98,7 @@ namespace MbD { void runPreDrag(); void runDragStep(std::shared_ptr>> dragParts); void runPostDrag(); + void restorePosRot(); void runKINEMATIC(); void initprincipalMassMarker(); std::shared_ptr spatialContainerAt(std::shared_ptr self, std::string& longname) const; diff --git a/OndselSolver/ASMTPart.cpp b/OndselSolver/ASMTPart.cpp index 3f804e4..fdf185a 100644 --- a/OndselSolver/ASMTPart.cpp +++ b/OndselSolver/ASMTPart.cpp @@ -126,6 +126,13 @@ void MbD::ASMTPart::createMbD(std::shared_ptr mbdSys, std::shared_ptr(mbdObject)->asFixed(); } +void MbD::ASMTPart::preMbDrunDragStep(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + auto mbdPart = std::static_pointer_cast(mbdObject); + mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length)); + mbdPart->qE(qEp()); +} + void MbD::ASMTPart::storeOnLevel(std::ofstream& os, size_t level) { storeOnLevelString(os, level, "Part"); diff --git a/OndselSolver/ASMTPart.h b/OndselSolver/ASMTPart.h index e3910da..2554f78 100644 --- a/OndselSolver/ASMTPart.h +++ b/OndselSolver/ASMTPart.h @@ -26,6 +26,7 @@ namespace MbD { FColDsptr omeOpO() override; ASMTPart* part() override; void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + void preMbDrunDragStep(std::shared_ptr mbdSys, std::shared_ptr mbdUnits); void storeOnLevel(std::ofstream& os, size_t level) override; void storeOnLevelMassMarker(std::ofstream& os, size_t level); void storeOnTimeSeries(std::ofstream& os) override; diff --git a/OndselSolver/ASMTSpatialContainer.cpp b/OndselSolver/ASMTSpatialContainer.cpp index c71e156..75a1cbf 100644 --- a/OndselSolver/ASMTSpatialContainer.cpp +++ b/OndselSolver/ASMTSpatialContainer.cpp @@ -376,9 +376,11 @@ void MbD::ASMTSpatialContainer::updateFromMbD() auto& aAPp = principalMassMarker->rotationMatrix; auto aAOP = aAOp->timesTransposeFullMatrix(aAPp); rotationMatrix = aAOP; + oldRotMat = rotationMatrix; auto rPcmO = aAOP->timesFullColumn(rPcmP); auto rOPO = rOcmO->minusFullColumn(rPcmO); position3D = rOPO; + oldPos3D = position3D; auto vOPO = vOcmO->minusFullColumn(omeOPO->cross(rPcmO)); velocity3D = vOPO; auto aOPO = aOcmO->minusFullColumn(alpOPO->cross(rPcmO))->minusFullColumn(omeOPO->cross(omeOPO->cross(rPcmO))); diff --git a/OndselSolver/ASMTSpatialItem.cpp b/OndselSolver/ASMTSpatialItem.cpp index 01a41ac..90fccee 100644 --- a/OndselSolver/ASMTSpatialItem.cpp +++ b/OndselSolver/ASMTSpatialItem.cpp @@ -169,3 +169,9 @@ FMatDsptr MbD::ASMTSpatialItem::getRotationMatrix(size_t i) bryantAngles->calc(); return bryantAngles->aA; } + +void MbD::ASMTSpatialItem::restorePosRot() +{ + position3D = oldPos3D; + rotationMatrix = oldRotMat; +} diff --git a/OndselSolver/ASMTSpatialItem.h b/OndselSolver/ASMTSpatialItem.h index 6ad1f89..655b5d7 100644 --- a/OndselSolver/ASMTSpatialItem.h +++ b/OndselSolver/ASMTSpatialItem.h @@ -36,6 +36,7 @@ namespace MbD { void storeOnLevelRotationMatrixRaw(std::ofstream& os, size_t level); FColDsptr getPosition3D(size_t i); FMatDsptr getRotationMatrix(size_t i); + void restorePosRot(); FColDsptr position3D = std::make_shared>(3); FMatDsptr rotationMatrix = std::make_shared>(ListListD{ @@ -43,7 +44,9 @@ namespace MbD { { 0, 1, 0 }, { 0, 0, 1 } }); - FRowDsptr xs, ys, zs, bryxs, bryys, bryzs; + FColDsptr oldPos3D = position3D; + FMatDsptr oldRotMat = rotationMatrix; + FRowDsptr xs, ys, zs, bryxs, bryys, bryzs; FRowDsptr inxs, inys, inzs, inbryxs, inbryys, inbryzs; }; diff --git a/OndselSolver/EulerParameters.h b/OndselSolver/EulerParameters.h index 04693dd..8df4a14 100644 --- a/OndselSolver/EulerParameters.h +++ b/OndselSolver/EulerParameters.h @@ -5,347 +5,405 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "EulerArray.h" #include "FullColumn.h" #include "FullMatrix.h" -namespace MbD { - - template - class EulerParameters : public EulerArray - { - //Quarternion = {q0, q1, q2, q3} - //EulerParameters = {qE1, qE2, qE3, qE4} is preferred because Smalltalk uses one-based indexing. - // q0 = qE4 - //Note: It is tempting to use quarternions in C++ because of zero-based indexing. - //Note: But that will make it harder to compare computation results with Smalltalk - //aA aB aC pApE - public: - EulerParameters() : EulerArray(4) {} - EulerParameters(size_t count) : EulerArray(count) {} - EulerParameters(size_t count, const T& value) : EulerArray(count, value) {} - EulerParameters(std::initializer_list list) : EulerArray{ list } {} - EulerParameters(FColDsptr axis, double theta) : EulerArray(4) { - auto halfTheta = theta / 2.0; - auto sinHalfTheta = std::sin(halfTheta); - auto cosHalfTheta = std::cos(halfTheta); - axis->normalizeSelf(); - this->atiputFullColumn(0, axis->times(sinHalfTheta)); - this->atiput(3, cosHalfTheta); - this->conditionSelf(); - this->initialize(); - this->calc(); - } +namespace MbD +{ - static std::shared_ptr>> ppApEpEtimesColumn(FColDsptr col); - static FMatDsptr pCpEtimesColumn(FColDsptr col); - static FMatDsptr pCTpEtimesColumn(FColDsptr col); - static std::shared_ptr>> ppApEpEtimesMatrix(FMatDsptr mat); +template +class EulerParameters: public EulerArray +{ + // Quarternion = {q0, q1, q2, q3} + // EulerParameters = {qE1, qE2, qE3, qE4} is preferred because Smalltalk uses one-based + // indexing. + // q0 = qE4 + // Note: It is tempting to use quarternions in C++ because of zero-based indexing. + // Note: But that will make it harder to compare computation results with Smalltalk + // aA aB aC pApE +public: + EulerParameters() + : EulerArray(4) + { + this->initialize(); + } + EulerParameters(size_t count) + : EulerArray(count) + { + this->initialize(); + } + EulerParameters(size_t count, const T& value) + : EulerArray(count, value) + { + this->initialize(); + } + EulerParameters(std::initializer_list list) + : EulerArray {list} + { + this->initialize(); + } + EulerParameters(FColDsptr axis, double theta) + : EulerArray(4) + { + auto halfTheta = theta / 2.0; + auto sinHalfTheta = std::sin(halfTheta); + auto cosHalfTheta = std::cos(halfTheta); + axis->normalizeSelf(); + this->atiputFullColumn(0, axis->times(sinHalfTheta)); + this->atiput(3, cosHalfTheta); + this->conditionSelf(); + this->initialize(); + this->calc(); + } + static std::shared_ptr>> ppApEpEtimesColumn(FColDsptr col); + static FMatDsptr pCpEtimesColumn(FColDsptr col); + static FMatDsptr pCTpEtimesColumn(FColDsptr col); + static std::shared_ptr>> ppApEpEtimesMatrix(FMatDsptr mat); - void initialize() override; - void calc() override; - void calcABC(); - void calcpApE(); - void conditionSelf() override; - FMatDsptr aA; - FMatDsptr aB; - FMatDsptr aC; - FColFMatDsptr pApE; - }; + void initialize() override; + void calc() override; + void calcABC(); + void calcpApE(); + void conditionSelf() override; + std::shared_ptr> times(T a); + std::shared_ptr> + plusFullColumn(std::shared_ptr> eulerPars); + std::shared_ptr> copy(); - template<> - inline FMatFColDsptr EulerParameters::ppApEpEtimesColumn(FColDsptr col) - { - double a2c0 = 2 * col->at(0); - double a2c1 = 2 * col->at(1); - double a2c2 = 2 * col->at(2); - double m2c0 = 0 - a2c0; - double m2c1 = 0 - a2c1; - double m2c2 = 0 - a2c2; - auto col00 = std::make_shared>(ListD{ a2c0, m2c1, m2c2 }); - auto col01 = std::make_shared>(ListD{ a2c1, a2c0, 0 }); - auto col02 = std::make_shared>(ListD{ a2c2, 0, a2c0 }); - auto col03 = std::make_shared>(ListD{ 0, m2c2, a2c1 }); - auto col11 = std::make_shared>(ListD{ m2c0, a2c1, m2c2 }); - auto col12 = std::make_shared>(ListD{ 0, a2c2, a2c1 }); - auto col13 = std::make_shared>(ListD{ a2c2, 0, m2c0 }); - auto col22 = std::make_shared>(ListD{ m2c0, m2c1, a2c2 }); - auto col23 = std::make_shared>(ListD{ m2c1, a2c0, 0 }); - auto col33 = std::make_shared>(ListD{ a2c0, a2c1, a2c2 }); - auto answer = std::make_shared>(4, 4); - auto& row0 = answer->at(0); - row0->at(0) = col00; - row0->at(1) = col01; - row0->at(2) = col02; - row0->at(3) = col03; - auto& row1 = answer->at(1); - row1->at(0) = col01; - row1->at(1) = col11; - row1->at(2) = col12; - row1->at(3) = col13; - auto& row2 = answer->at(2); - row2->at(0) = col02; - row2->at(1) = col12; - row2->at(2) = col22; - row2->at(3) = col23; - auto& row3 = answer->at(3); - row3->at(0) = col03; - row3->at(1) = col13; - row3->at(2) = col23; - row3->at(3) = col33; - return answer; - } + FMatDsptr aA; + FMatDsptr aB; + FMatDsptr aC; + FColFMatDsptr pApE; +}; - template<> - inline FMatDsptr EulerParameters::pCpEtimesColumn(FColDsptr col) - { - //"col size = 4." - auto c0 = col->at(0); - auto c1 = col->at(1); - auto c2 = col->at(2); - auto mc0 = -c0; - auto mc1 = -c1; - auto mc2 = -c2; - auto mc3 = -col->at(3); - auto answer = std::make_shared>(3, 4); - auto& row0 = answer->at(0); - auto& row1 = answer->at(1); - auto& row2 = answer->at(2); - row0->atiput(0, mc3); - row0->atiput(1, mc2); - row0->atiput(2, c1); - row0->atiput(3, c0); - row1->atiput(0, c2); - row1->atiput(1, mc3); - row1->atiput(2, mc0); - row1->atiput(3, c1); - row2->atiput(0, mc1); - row2->atiput(1, c0); - row2->atiput(2, mc3); - row2->atiput(3, c2); - return answer; - } +template<> +inline FMatFColDsptr EulerParameters::ppApEpEtimesColumn(FColDsptr col) +{ + double a2c0 = 2 * col->at(0); + double a2c1 = 2 * col->at(1); + double a2c2 = 2 * col->at(2); + double m2c0 = 0 - a2c0; + double m2c1 = 0 - a2c1; + double m2c2 = 0 - a2c2; + auto col00 = std::make_shared>(ListD {a2c0, m2c1, m2c2}); + auto col01 = std::make_shared>(ListD {a2c1, a2c0, 0}); + auto col02 = std::make_shared>(ListD {a2c2, 0, a2c0}); + auto col03 = std::make_shared>(ListD {0, m2c2, a2c1}); + auto col11 = std::make_shared>(ListD {m2c0, a2c1, m2c2}); + auto col12 = std::make_shared>(ListD {0, a2c2, a2c1}); + auto col13 = std::make_shared>(ListD {a2c2, 0, m2c0}); + auto col22 = std::make_shared>(ListD {m2c0, m2c1, a2c2}); + auto col23 = std::make_shared>(ListD {m2c1, a2c0, 0}); + auto col33 = std::make_shared>(ListD {a2c0, a2c1, a2c2}); + auto answer = std::make_shared>(4, 4); + auto& row0 = answer->at(0); + row0->at(0) = col00; + row0->at(1) = col01; + row0->at(2) = col02; + row0->at(3) = col03; + auto& row1 = answer->at(1); + row1->at(0) = col01; + row1->at(1) = col11; + row1->at(2) = col12; + row1->at(3) = col13; + auto& row2 = answer->at(2); + row2->at(0) = col02; + row2->at(1) = col12; + row2->at(2) = col22; + row2->at(3) = col23; + auto& row3 = answer->at(3); + row3->at(0) = col03; + row3->at(1) = col13; + row3->at(2) = col23; + row3->at(3) = col33; + return answer; +} - template - inline FMatDsptr EulerParameters::pCTpEtimesColumn(FColDsptr col) - { - //"col size = 3." - auto c0 = col->at(0); - auto c1 = col->at(1); - auto c2 = col->at(2); - auto mc0 = -c0; - auto mc1 = -c1; - auto mc2 = -c2; - auto answer = std::make_shared>(4, 4); - auto& row0 = answer->at(0); - auto& row1 = answer->at(1); - auto& row2 = answer->at(2); - auto& row3 = answer->at(3); - row0->atiput(0, 0.0); - row0->atiput(1, c2); - row0->atiput(2, mc1); - row0->atiput(3, c0); - row1->atiput(0, mc2); - row1->atiput(1, 0.0); - row1->atiput(2, c0); - row1->atiput(3, c1); - row2->atiput(0, c1); - row2->atiput(1, mc0); - row2->atiput(2, 0.0); - row2->atiput(3, c2); - row3->atiput(0, mc0); - row3->atiput(1, mc1); - row3->atiput(2, mc2); - row3->atiput(3, 0.0); - return answer; - } +template<> +inline FMatDsptr EulerParameters::pCpEtimesColumn(FColDsptr col) +{ + //"col size = 4." + auto c0 = col->at(0); + auto c1 = col->at(1); + auto c2 = col->at(2); + auto mc0 = -c0; + auto mc1 = -c1; + auto mc2 = -c2; + auto mc3 = -col->at(3); + auto answer = std::make_shared>(3, 4); + auto& row0 = answer->at(0); + auto& row1 = answer->at(1); + auto& row2 = answer->at(2); + row0->atiput(0, mc3); + row0->atiput(1, mc2); + row0->atiput(2, c1); + row0->atiput(3, c0); + row1->atiput(0, c2); + row1->atiput(1, mc3); + row1->atiput(2, mc0); + row1->atiput(3, c1); + row2->atiput(0, mc1); + row2->atiput(1, c0); + row2->atiput(2, mc3); + row2->atiput(3, c2); + return answer; +} - template<> - inline FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) - { - FRowDsptr a2m0 = mat->at(0)->times(2.0); - FRowDsptr a2m1 = mat->at(1)->times(2.0); - FRowDsptr a2m2 = mat->at(2)->times(2.0); - FRowDsptr m2m0 = a2m0->negated(); - FRowDsptr m2m1 = a2m1->negated(); - FRowDsptr m2m2 = a2m2->negated(); - FRowDsptr zero = std::make_shared>(3, 0.0); - auto mat00 = std::make_shared>(ListFRD{ a2m0, m2m1, m2m2 }); - auto mat01 = std::make_shared>(ListFRD{ a2m1, a2m0, zero }); - auto mat02 = std::make_shared>(ListFRD{ a2m2, zero, a2m0 }); - auto mat03 = std::make_shared>(ListFRD{ zero, m2m2, a2m1 }); - auto mat11 = std::make_shared>(ListFRD{ m2m0, a2m1, m2m2 }); - auto mat12 = std::make_shared>(ListFRD{ zero, a2m2, a2m1 }); - auto mat13 = std::make_shared>(ListFRD{ a2m2, zero, m2m0 }); - auto mat22 = std::make_shared>(ListFRD{ m2m0, m2m1, a2m2 }); - auto mat23 = std::make_shared>(ListFRD{ m2m1, a2m0, zero }); - auto mat33 = std::make_shared>(ListFRD{ a2m0, a2m1, a2m2 }); - auto answer = std::make_shared>(4, 4); - auto& row0 = answer->at(0); - row0->at(0) = mat00; - row0->at(1) = mat01; - row0->at(2) = mat02; - row0->at(3) = mat03; - auto& row1 = answer->at(1); - row1->at(0) = mat01; - row1->at(1) = mat11; - row1->at(2) = mat12; - row1->at(3) = mat13; - auto& row2 = answer->at(2); - row2->at(0) = mat02; - row2->at(1) = mat12; - row2->at(2) = mat22; - row2->at(3) = mat23; - auto& row3 = answer->at(3); - row3->at(0) = mat03; - row3->at(1) = mat13; - row3->at(2) = mat23; - row3->at(3) = mat33; - return answer; - } +template +inline FMatDsptr EulerParameters::pCTpEtimesColumn(FColDsptr col) +{ + //"col size = 3." + auto c0 = col->at(0); + auto c1 = col->at(1); + auto c2 = col->at(2); + auto mc0 = -c0; + auto mc1 = -c1; + auto mc2 = -c2; + auto answer = std::make_shared>(4, 4); + auto& row0 = answer->at(0); + auto& row1 = answer->at(1); + auto& row2 = answer->at(2); + auto& row3 = answer->at(3); + row0->atiput(0, 0.0); + row0->atiput(1, c2); + row0->atiput(2, mc1); + row0->atiput(3, c0); + row1->atiput(0, mc2); + row1->atiput(1, 0.0); + row1->atiput(2, c0); + row1->atiput(3, c1); + row2->atiput(0, c1); + row2->atiput(1, mc0); + row2->atiput(2, 0.0); + row2->atiput(3, c2); + row3->atiput(0, mc0); + row3->atiput(1, mc1); + row3->atiput(2, mc2); + row3->atiput(3, 0.0); + return answer; +} - template<> - inline void EulerParameters::initialize() - { - aA = FullMatrix::identitysptr(3); - aB = std::make_shared>(3, 4); - aC = std::make_shared>(3, 4); - pApE = std::make_shared>(4); - for (size_t i = 0; i < 4; i++) - { - pApE->at(i) = std::make_shared>(3, 3); - } - } - template - inline void EulerParameters::calc() - { - this->calcABC(); - this->calcpApE(); - } - template<> - inline void EulerParameters::calcABC() - { - double aE0 = this->at(0); - double aE1 = this->at(1); - double aE2 = this->at(2); - double aE3 = this->at(3); - double mE0 = -aE0; - double mE1 = -aE1; - double mE2 = -aE2; - FRowDsptr aBi; - aBi = aB->at(0); - aBi->at(0) = aE3; - aBi->at(1) = mE2; - aBi->at(2) = aE1; - aBi->at(3) = mE0; - aBi = aB->at(1); - aBi->at(0) = aE2; - aBi->at(1) = aE3; - aBi->at(2) = mE0; - aBi->at(3) = mE1; - aBi = aB->at(2); - aBi->at(0) = mE1; - aBi->at(1) = aE0; - aBi->at(2) = aE3; - aBi->at(3) = mE2; - FRowDsptr aCi; - aCi = aC->at(0); - aCi->at(0) = aE3; - aCi->at(1) = aE2; - aCi->at(2) = mE1; - aCi->at(3) = mE0; - aCi = aC->at(1); - aCi->at(0) = mE2; - aCi->at(1) = aE3; - aCi->at(2) = aE0; - aCi->at(3) = mE1; - aCi = aC->at(2); - aCi->at(0) = aE1; - aCi->at(1) = mE0; - aCi->at(2) = aE3; - aCi->at(3) = mE2; +template<> +inline FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) +{ + FRowDsptr a2m0 = mat->at(0)->times(2.0); + FRowDsptr a2m1 = mat->at(1)->times(2.0); + FRowDsptr a2m2 = mat->at(2)->times(2.0); + FRowDsptr m2m0 = a2m0->negated(); + FRowDsptr m2m1 = a2m1->negated(); + FRowDsptr m2m2 = a2m2->negated(); + FRowDsptr zero = std::make_shared>(3, 0.0); + auto mat00 = std::make_shared>(ListFRD {a2m0, m2m1, m2m2}); + auto mat01 = std::make_shared>(ListFRD {a2m1, a2m0, zero}); + auto mat02 = std::make_shared>(ListFRD {a2m2, zero, a2m0}); + auto mat03 = std::make_shared>(ListFRD {zero, m2m2, a2m1}); + auto mat11 = std::make_shared>(ListFRD {m2m0, a2m1, m2m2}); + auto mat12 = std::make_shared>(ListFRD {zero, a2m2, a2m1}); + auto mat13 = std::make_shared>(ListFRD {a2m2, zero, m2m0}); + auto mat22 = std::make_shared>(ListFRD {m2m0, m2m1, a2m2}); + auto mat23 = std::make_shared>(ListFRD {m2m1, a2m0, zero}); + auto mat33 = std::make_shared>(ListFRD {a2m0, a2m1, a2m2}); + auto answer = std::make_shared>(4, 4); + auto& row0 = answer->at(0); + row0->at(0) = mat00; + row0->at(1) = mat01; + row0->at(2) = mat02; + row0->at(3) = mat03; + auto& row1 = answer->at(1); + row1->at(0) = mat01; + row1->at(1) = mat11; + row1->at(2) = mat12; + row1->at(3) = mat13; + auto& row2 = answer->at(2); + row2->at(0) = mat02; + row2->at(1) = mat12; + row2->at(2) = mat22; + row2->at(3) = mat23; + auto& row3 = answer->at(3); + row3->at(0) = mat03; + row3->at(1) = mat13; + row3->at(2) = mat23; + row3->at(3) = mat33; + return answer; +} - aA = aB->timesTransposeFullMatrix(aC); - } - template<> - inline void EulerParameters::calcpApE() - { - double a2E0 = 2.0 * (this->at(0)); - double a2E1 = 2.0 * (this->at(1)); - double a2E2 = 2.0 * (this->at(2)); - double a2E3 = 2.0 * (this->at(3)); - double m2E0 = -a2E0; - double m2E1 = -a2E1; - double m2E2 = -a2E2; - double m2E3 = -a2E3; - FMatDsptr pApEk; - pApEk = pApE->at(0); - FRowDsptr pAipEk; - pAipEk = pApEk->at(0); - pAipEk->at(0) = a2E0; - pAipEk->at(1) = a2E1; - pAipEk->at(2) = a2E2; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E1; - pAipEk->at(1) = m2E0; - pAipEk->at(2) = m2E3; - pAipEk = pApEk->at(2); - pAipEk->at(0) = a2E2; - pAipEk->at(1) = a2E3; - pAipEk->at(2) = m2E0; - // - pApEk = pApE->at(1); - pAipEk = pApEk->at(0); - pAipEk->at(0) = m2E1; - pAipEk->at(1) = a2E0; - pAipEk->at(2) = a2E3; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E0; - pAipEk->at(1) = a2E1; - pAipEk->at(2) = a2E2; - pAipEk = pApEk->at(2); - pAipEk->at(0) = m2E3; - pAipEk->at(1) = a2E2; - pAipEk->at(2) = m2E1; - // - pApEk = pApE->at(2); - pAipEk = pApEk->at(0); - pAipEk->at(0) = m2E2; - pAipEk->at(1) = m2E3; - pAipEk->at(2) = a2E0; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E3; - pAipEk->at(1) = m2E2; - pAipEk->at(2) = a2E1; - pAipEk = pApEk->at(2); - pAipEk->at(0) = a2E0; - pAipEk->at(1) = a2E1; - pAipEk->at(2) = a2E2; - // - pApEk = pApE->at(3); - pAipEk = pApEk->at(0); - pAipEk->at(0) = a2E3; - pAipEk->at(1) = m2E2; - pAipEk->at(2) = a2E1; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E2; - pAipEk->at(1) = a2E3; - pAipEk->at(2) = m2E0; - pAipEk = pApEk->at(2); - pAipEk->at(0) = m2E1; - pAipEk->at(1) = a2E0; - pAipEk->at(2) = a2E3; - } - template - inline void EulerParameters::conditionSelf() - { - EulerArray::conditionSelf(); - this->normalizeSelf(); - } +template<> +inline void EulerParameters::initialize() +{ + aA = FullMatrix::identitysptr(3); + aB = std::make_shared>(3, 4); + aC = std::make_shared>(3, 4); + pApE = std::make_shared>(4); + for (size_t i = 0; i < 4; i++) { + pApE->at(i) = std::make_shared>(3, 3); + } +} +template +inline void EulerParameters::calc() +{ + this->calcABC(); + this->calcpApE(); } +template<> +inline void EulerParameters::calcABC() +{ + double aE0 = this->at(0); + double aE1 = this->at(1); + double aE2 = this->at(2); + double aE3 = this->at(3); + double mE0 = -aE0; + double mE1 = -aE1; + double mE2 = -aE2; + FRowDsptr aBi; + aBi = aB->at(0); + aBi->at(0) = aE3; + aBi->at(1) = mE2; + aBi->at(2) = aE1; + aBi->at(3) = mE0; + aBi = aB->at(1); + aBi->at(0) = aE2; + aBi->at(1) = aE3; + aBi->at(2) = mE0; + aBi->at(3) = mE1; + aBi = aB->at(2); + aBi->at(0) = mE1; + aBi->at(1) = aE0; + aBi->at(2) = aE3; + aBi->at(3) = mE2; + FRowDsptr aCi; + aCi = aC->at(0); + aCi->at(0) = aE3; + aCi->at(1) = aE2; + aCi->at(2) = mE1; + aCi->at(3) = mE0; + aCi = aC->at(1); + aCi->at(0) = mE2; + aCi->at(1) = aE3; + aCi->at(2) = aE0; + aCi->at(3) = mE1; + aCi = aC->at(2); + aCi->at(0) = aE1; + aCi->at(1) = mE0; + aCi->at(2) = aE3; + aCi->at(3) = mE2; + aA = aB->timesTransposeFullMatrix(aC); +} +template<> +inline void EulerParameters::calcpApE() +{ + double a2E0 = 2.0 * (this->at(0)); + double a2E1 = 2.0 * (this->at(1)); + double a2E2 = 2.0 * (this->at(2)); + double a2E3 = 2.0 * (this->at(3)); + double m2E0 = -a2E0; + double m2E1 = -a2E1; + double m2E2 = -a2E2; + double m2E3 = -a2E3; + FMatDsptr pApEk; + pApEk = pApE->at(0); + FRowDsptr pAipEk; + pAipEk = pApEk->at(0); + pAipEk->at(0) = a2E0; + pAipEk->at(1) = a2E1; + pAipEk->at(2) = a2E2; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E1; + pAipEk->at(1) = m2E0; + pAipEk->at(2) = m2E3; + pAipEk = pApEk->at(2); + pAipEk->at(0) = a2E2; + pAipEk->at(1) = a2E3; + pAipEk->at(2) = m2E0; + // + pApEk = pApE->at(1); + pAipEk = pApEk->at(0); + pAipEk->at(0) = m2E1; + pAipEk->at(1) = a2E0; + pAipEk->at(2) = a2E3; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E0; + pAipEk->at(1) = a2E1; + pAipEk->at(2) = a2E2; + pAipEk = pApEk->at(2); + pAipEk->at(0) = m2E3; + pAipEk->at(1) = a2E2; + pAipEk->at(2) = m2E1; + // + pApEk = pApE->at(2); + pAipEk = pApEk->at(0); + pAipEk->at(0) = m2E2; + pAipEk->at(1) = m2E3; + pAipEk->at(2) = a2E0; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E3; + pAipEk->at(1) = m2E2; + pAipEk->at(2) = a2E1; + pAipEk = pApEk->at(2); + pAipEk->at(0) = a2E0; + pAipEk->at(1) = a2E1; + pAipEk->at(2) = a2E2; + // + pApEk = pApE->at(3); + pAipEk = pApEk->at(0); + pAipEk->at(0) = a2E3; + pAipEk->at(1) = m2E2; + pAipEk->at(2) = a2E1; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E2; + pAipEk->at(1) = a2E3; + pAipEk->at(2) = m2E0; + pAipEk = pApEk->at(2); + pAipEk->at(0) = m2E1; + pAipEk->at(1) = a2E0; + pAipEk->at(2) = a2E3; +} +template +inline void EulerParameters::conditionSelf() +{ + EulerArray::conditionSelf(); + this->normalizeSelf(); +} +template<> +inline std::shared_ptr> EulerParameters::times(double a) +{ + auto n = this->size(); + auto answer = std::make_shared>(n); + for (size_t i = 0; i < n; i++) { + answer->at(i) = this->at(i) * a; + } + return answer; +} +template +inline std::shared_ptr> EulerParameters::times(T) +{ + assert(false); +} +template +inline std::shared_ptr> +EulerParameters::plusFullColumn(std::shared_ptr> eulerPars) +{ + auto n = this->size(); + auto answer = std::make_shared>(n); + for (size_t i = 0; i < n; i++) { + answer->at(i) = this->at(i) + eulerPars->at(i); + } + return answer; +} +template +inline std::shared_ptr> EulerParameters::copy() +{ + auto n = this->size(); + auto answer = std::make_shared>(n); + for (size_t i = 0; i < n; i++) { + answer->at(i) = this->at(i); + } + return answer; +} +} // namespace MbD diff --git a/OndselSolver/ExternalSystem.cpp b/OndselSolver/ExternalSystem.cpp index 4982958..399b763 100644 --- a/OndselSolver/ExternalSystem.cpp +++ b/OndselSolver/ExternalSystem.cpp @@ -26,6 +26,11 @@ void MbD::ExternalSystem::preMbDrun(std::shared_ptr mbdSys) } } +void MbD::ExternalSystem::preMbDrunDragStep(std::shared_ptr mbdSys, std::shared_ptr>> dragParts) +{ + asmtAssembly->preMbDrunDragStep(mbdSys, dragParts); +} + void MbD::ExternalSystem::updateFromMbD() { if (cadSystem) { diff --git a/OndselSolver/ExternalSystem.h b/OndselSolver/ExternalSystem.h index 743f044..65730ec 100644 --- a/OndselSolver/ExternalSystem.h +++ b/OndselSolver/ExternalSystem.h @@ -14,6 +14,7 @@ //#include "CADSystem.h" //#include "ASMTAssembly.h" +#include "Part.h" namespace MbD { class CADSystem; @@ -25,6 +26,7 @@ namespace MbD { // public: void preMbDrun(std::shared_ptr mbdSys); + void preMbDrunDragStep(std::shared_ptr mbdSys, std::shared_ptr>> dragParts); void updateFromMbD(); void outputFor(AnalysisType type); void logString(std::string& str); diff --git a/OndselSolver/OndselSolver.vcxproj b/OndselSolver/OndselSolver.vcxproj index b6cfb35..7b94ddc 100644 --- a/OndselSolver/OndselSolver.vcxproj +++ b/OndselSolver/OndselSolver.vcxproj @@ -778,27 +778,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -808,6 +840,10 @@ + + + + diff --git a/OndselSolver/OndselSolver.vcxproj.filters b/OndselSolver/OndselSolver.vcxproj.filters index a6c5883..980f2ce 100644 --- a/OndselSolver/OndselSolver.vcxproj.filters +++ b/OndselSolver/OndselSolver.vcxproj.filters @@ -1954,6 +1954,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1963,5 +1995,9 @@ + + + + \ No newline at end of file diff --git a/OndselSolver/System.cpp b/OndselSolver/System.cpp index b87e7d2..b88dc63 100644 --- a/OndselSolver/System.cpp +++ b/OndselSolver/System.cpp @@ -125,8 +125,9 @@ void MbD::System::runPreDrag(std::shared_ptr self) externalSystem->updateFromMbD(); } -void MbD::System::runDragStep(std::shared_ptr>> dragParts) +void MbD::System::runDragStep(std::shared_ptr self, std::shared_ptr>> dragParts) { + externalSystem->preMbDrunDragStep(self, dragParts); partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postInput(); }); systemSolver->runDragStep(dragParts); externalSystem->updateFromMbD(); diff --git a/OndselSolver/System.h b/OndselSolver/System.h index d9212fb..19aa1a2 100644 --- a/OndselSolver/System.h +++ b/OndselSolver/System.h @@ -45,7 +45,7 @@ namespace MbD { void initializeGlobally() override; void clear(); void runPreDrag(std::shared_ptr self); - void runDragStep(std::shared_ptr>> dragParts); + void runDragStep(std::shared_ptr self, std::shared_ptr>> dragParts); void runKINEMATIC(std::shared_ptr self); std::shared_ptr> discontinuitiesAtIC(); void jointsMotionsDo(const std::function )>& f); diff --git a/OndselSolver/SystemNewtonRaphson.cpp b/OndselSolver/SystemNewtonRaphson.cpp index e94170e..3ad1fc5 100644 --- a/OndselSolver/SystemNewtonRaphson.cpp +++ b/OndselSolver/SystemNewtonRaphson.cpp @@ -76,7 +76,7 @@ void SystemNewtonRaphson::handleSingularMatrix() if (str.find("GESpMatParPvPrecise") != std::string::npos) { str = "MbD: Singular Matrix Error. "; system->logString(str); - matrixSolver = this->matrixSolverClassNew(); + matrixSolver->throwSingularMatrixError("SystemNewtonRaphson"); } else { assert(false); diff --git a/OndselSolver/VelSolver.cpp b/OndselSolver/VelSolver.cpp index 72b5f1e..3987b47 100644 --- a/OndselSolver/VelSolver.cpp +++ b/OndselSolver/VelSolver.cpp @@ -34,8 +34,8 @@ void VelSolver::handleSingularMatrix() str = typeid(r).name(); if (str.find("GESpMatParPvPrecise") != std::string::npos) { this->logSingularMatrixMessage(); - matrixSolver = this->matrixSolverClassNew(); - } + matrixSolver->throwSingularMatrixError("VelSolver"); + } else { assert(false); } diff --git a/OndselSolver/runDragStep.asmt b/OndselSolver/runDragStep.asmt new file mode 100644 index 0000000..9bced4c --- /dev/null +++ b/OndselSolver/runDragStep.asmt @@ -0,0 +1,452 @@ +OndselSolver +Assembly + Notes + (Text string: 'CAD: Copyright (C) 2000-2004, Aik-Siong Koh, All Rights Reserved.The piston crank is the most common mechanism to convert rotary motion to reciprocating motion or vice versa. A crank connects to the ground with a revolute joint. Its other end is connected to the connecting rod. And the connecting rod is connected to the piston which slides along an axis on the ground. The crank is given rotary motion causing the piston to slides back and forth is a straight line. Units are SI units. Angles are in radians.If the instructions below are too brief, refer to the Notes in projectile.asm and circular.asm.To load the example for a quick look:Click File/Open/Assembly/ to get a dialog. Enter *.asm for a list of assemblies. Select piston.asm. To create the assembly from scratch:To create crank, connection rod and piston:Create an empty assembly and populate it with two rods (Assembly1Part1, Assembly1Part2) and one cylinder (Assembly1Part3). The rods have dimensions (1.0d, 0.2d, 0.1d) and (1.5d, 0.2d, 0.1d). The cylinder has radius (0.5d) and height (1.0d). Arrange them from bottom up away from the origin. To mark joint attachment points:On the ground, create a marker (Assembly1Marker1) at (0.0d, 0.0d, 0.0d) and another (Assembly1Marker2) at (3.0d, 0.0d, 0.0d). On the first rod, create a marker (Assembly1Part1Marker1) at (0.1d, 0.1d, 0.0d) and another (Assembly1Part1Marker2) at (0.9d, 0.1d, 0.0d) relative to the z-face. On the second rod, create a marker (Assembly1Part2Marker1) at (0.1d, 0.1d, -0.1d) and another (Assembly1Part2Marker2) at (1.4d, 0.1d, -0.1d) relative to the z-face. On the cylinder, create a marker (Assembly1Part3Marker1) at (0.0d, 0.0d, 0.0d) and another (Assembly1Part3Marker2) at (0.0d, 0.0d, -1.0d) relative to the z-face. Tilt the cylinder a little to get a good view of (Assembly1Part3Marker2). RightClick/Rotate/ over it to rotate the marker (90.0d) degrees about the x-axis.Tilt the cylinder upright to help the solver assemble the system later.To create the joints:Connect (Assembly1Marker1) to (Assembly1Part1Marker1) with revolute joint (Assembly1Joint1).Connect (Assembly1Part1Marker2) to (Assembly1Part2Marker1) with revolute joint (Assembly1Joint2).Connect (Assembly1Part2Marker2) to (Assembly1Part3Marker2) with revolute joint (Assembly1Joint3).Connect (Assembly1Marker2) to (Assembly1Part3Marker1) with translational joint (Assembly1Joint3).The translational joint keeps the marker z-axes parallel and colinear. Only relative translation along the z-axis is permitted.To apply motion to the crank:Apply rotation motion (Assembly1Motion1) to (Assembly1Joint1). Enter 2.0d*pi*time.The assembly is now ready for simulation, animation and plotting.' runs: (Core.RunArray runs: #(514 63 14 5 12 1 2 37 89 10 4 36 1 43 295 32 848 21 517 29 151) values: #(nil #underline #(#underline #bold) #underline #(#underline #bold) #underline nil #(#bold #large) nil #bold nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil))) + Name + Assembly1 + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + RefPoints + RefPoint + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker1 + Position3D + 0 3 0 + RotationMatrix + 1 0 0 + 0 0 1 + 0 -1 0 + RefPoint + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker2 + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + RefCurves + RefSurfaces + Parts + Part + Name + Part1 + Position3D + -0.099999999999995204 -0.10000000000000477 -0.10000000000000001 + RotationMatrix + 1 -4.7573992180162559e-14 0 + 4.7573992180162559e-14 1 0 + 0 0 1 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + FeatureOrder + PrincipalMassMarker + Name + MassMarker + Position3D + 0.5 0.10000000000000001 0.050000000000000003 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Mass + 0.20000000000000001 + MomentOfInertias + 0.00083333333333333003 0.016833333333332999 0.017333333333332999 + Density + 10 + RefPoints + RefPoint + Position3D + 0 0 0.10000000000000001 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker1 + Position3D + 0.10000000000000001 0.10000000000000001 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + RefPoint + Position3D + 0 0 0.10000000000000001 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker2 + Position3D + 0.90000000000000002 0.10000000000000001 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + RefCurves + RefSurfaces + Part + Name + Part2 + Position3D + 0.94036115973815071 -0.017284236661189611 1.3877787807814457e-17 + RotationMatrix + -0.61538461538461542 -0.78822698199689212 -3.1196969183971049e-17 + 0.78822698199689212 -0.61538461538461542 3.9959225909413745e-17 + -5.0695074923952936e-17 -9.2444637330587321e-33 1 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + FeatureOrder + PrincipalMassMarker + Name + MassMarker + Position3D + 0.75 0.10000000000000001 0.050000000000000003 + RotationMatrix + 1 -2.7755575615629002e-16 0 + 2.7755575615629002e-16 1 0 + 0 0 1 + Mass + 0.29999999999999999 + MomentOfInertias + 0.00125 0.056500000000000002 0.057250000000000002 + Density + 10 + RefPoints + RefPoint + Position3D + 0 0 0.10000000000000001 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker1 + Position3D + 0.10000000000000001 0.10000000000000001 -0.10000000000000001 + RotationMatrix + 1 1.6433823482156999e-50 6.1629758220391999e-33 + 1.9277988905447e-49 1 1.055150120668e-32 + 6.1629758220391999e-33 8.8305116654984998e-33 1 + RefPoint + Position3D + -7.0256300777060995e-17 -1.0408340855861e-17 0.10000000000000001 + RotationMatrix + 1 1.7347234759768e-18 -6.9388939039071999e-18 + 1.7347234759768e-18 1 0 + -6.9388939039071999e-18 0 1 + Markers + Marker + Name + Marker2 + Position3D + 1.3999999999999999 0.10000000000000001 -0.10000000000000001 + RotationMatrix + 1 1.9417266172264999e-33 1.873501354055e-16 + 1.4257315131995e-48 1 4.1633363423443e-17 + -2.5673907444456001e-16 -4.1633363423443e-17 1 + RefCurves + RefSurfaces + Part + Name + Part3 + Position3D + -2.3989912302823933e-18 1.0246950765959977 -7.3873102870091778e-17 + RotationMatrix + 1 1.7503581204812994e-16 2.2204460492503131e-16 + -2.2204460492503131e-16 6.1232339957367623e-17 1 + 1.7503581204812992e-16 -1 6.123233995736766e-17 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + FeatureOrder + PrincipalMassMarker + Name + MassMarker + Position3D + -7.9328390680451997e-18 2.9323172983666999e-17 0.5 + RotationMatrix + 9.2444637330587006e-33 1 -1.0785207688569e-32 + 9.9703461330478005e-65 1.0785207688569e-32 1 + 1 -9.2444637330587006e-33 0 + Mass + 7.6536686473018003 + MomentOfInertias + 0.93243354610287998 1.1040224936598999 1.1040224936598999 + Density + 10 + RefPoints + RefPoint + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 -1 0 + 0 0 -1 + Markers + Marker + Name + Marker1 + Position3D + 0 0 0 + RotationMatrix + 1 2.1223636732195001e-32 -2.4651903288157002e-32 + -2.4651903288157002e-32 -2.2204460492503e-16 1 + -1.1179465652455999e-32 -1 -2.2204460492503e-16 + RefPoint + Position3D + 1.0408340855861e-17 5.5511151231258e-17 1 + RotationMatrix + 1 -6.9388939039071999e-18 6.9388939039071999e-18 + -6.9388939039071999e-18 1 0 + 6.9388939039071999e-18 0 1 + Markers + Marker + Name + Marker2 + Position3D + 0 0 0 + RotationMatrix + 1 -4.1633363423442002e-17 4.1633363423442002e-17 + -4.1633363423442002e-17 1 -4.9303806576313002e-32 + 4.1633363423442002e-17 -3.6977854932235e-32 1 + RefCurves + RefSurfaces + KinematicIJs + ConstraintSets + Joints + RevoluteJoint + Name + Joint1 + MarkerI + /Assembly1/Marker2 + MarkerJ + /Assembly1/Part1/Marker1 + RevoluteJoint + Name + Joint2 + MarkerI + /Assembly1/Part1/Marker2 + MarkerJ + /Assembly1/Part2/Marker1 + RevoluteJoint + Name + Joint3 + MarkerI + /Assembly1/Part2/Marker2 + MarkerJ + /Assembly1/Part3/Marker1 + CylindricalJoint + Name + Joint4 + MarkerI + /Assembly1/Part3/Marker2 + MarkerJ + /Assembly1/Marker1 + Motions + Limits + RotationLimit + Name + Limit1 + MarkerI + /Assembly1/Marker2 + MarkerJ + /Assembly1/Part1/Marker1 + MotionJoint + + Limit + 30.0*pi/180.0 + Type + => + Tol + 1.0e-9 + TranslationLimit + Name + Limit2 + MarkerI + /Assembly1/Part3/Marker2 + MarkerJ + /Assembly1/Marker1 + MotionJoint + + Limit + 1.2 + Type + =< + Tol + 1.0e-9 + GeneralConstraintSets + ForceTorques + ConstantGravity + 0 -9.8100000000000005 0 + SimulationParameters + tstart + 0 + tend + 0 + hmin + 1.0000000000000001e-09 + hmax + 1 + hout + 0.040000000000000001 + errorTol + 9.9999999999999995e-07 + AnimationParameters + nframe + 26 + icurrent + 1 + istart + 1 + iend + 26 + isForward + true + framesPerSecond + 30 +TimeSeries +Number Input 1 +Time Input 0 +AssemblySeries /Assembly1 +X 0 0 +Y 0 0 +Z 0 0 +Bryantx -0 -0 +Bryanty 0 0 +Bryantz -0 -0 +VX 0 0 +VY 0 0 +VZ 0 0 +OmegaX 0 0 +OmegaY 0 0 +OmegaZ 0 0 +AX 0 0 +AY 0 0 +AZ 0 0 +AlphaX 0 0 +AlphaY 0 0 +AlphaZ 0 0 +PartSeries /Assembly1/Part1 +X -0.099999999999995204 -0.036602540378443793 +Y -0.10000000000000477 -0.13660254037844385 +Z -0.10000000000000001 -0.10000000000000001 +Bryantx -0 -0 +Bryanty 0 0 +Bryantz 4.7573992180162559e-14 0.52359877559829882 +VX 0 0 +VY 0 0 +VZ 0 0 +OmegaX 0 0 +OmegaY 0 0 +OmegaZ 0 0 +AX 0 0 +AY 0 0 +AZ 0 0 +AlphaX 0 0 +AlphaY 0 0 +AlphaZ 0 0 +PartSeries /Assembly1/Part2 +X 0.94036115973815071 0.83072957864505503 +Y -0.017284236661189611 0.36867848638673484 +Z 1.3877787807814457e-17 0 +Bryantx -3.9959225909413745e-17 -2.0810386949440074e-17 +Bryanty -3.1196969183971049e-17 -1.3107144553308558e-17 +Bryantz 2.2336701504450325 2.132866129800524 +VX 0 0 +VY 0 0 +VZ 0 0 +OmegaX 0 0 +OmegaY 0 0 +OmegaZ 0 0 +AX 0 0 +AY 0 0 +AZ 0 0 +AlphaX 0 0 +AlphaY 0 0 +AlphaZ 0 0 +PartSeries /Assembly1/Part3 +X -2.3989912302823933e-18 1.7347234759767844e-16 +Y 1.0246950765959977 1.5 +Z -7.3873102870091778e-17 -5.0638723398281993e-17 +Bryantx -1.5707963267948966 -1.5707963267948966 +Bryanty 2.2204460492503131e-16 -2.2204460492503131e-16 +Bryantz -1.7503581204812994e-16 -1.4448350946155655e-16 +VX 0 0 +VY 0 0 +VZ 0 0 +OmegaX 0 0 +OmegaY 0 0 +OmegaZ 0 0 +AX 0 0 +AY 0 0 +AZ 0 0 +AlphaX 0 0 +AlphaY 0 0 +AlphaZ 0 0 +RevoluteJointSeries /Assembly1/Joint1 +FXonI -1.096976176150689e-09 90532.831584811574 +FYonI -9.8692654817300584e-12 -145496.38290754412 +FZonI -1.7324150085031064e-11 -91.459335864185263 +TXonI -3.2008046877209379e-11 -100.29134722649302 +TYonI 2.1791997674716458e-11 16.628023833341494 +TZonI 0 0 +RevoluteJointSeries /Assembly1/Joint2 +FXonI -1.0969761761506894e-09 89842.257554541997 +FYonI 5.0642766158168451e-10 -144068.4477002006 +FZonI -1.7324150085031064e-11 -91.459335864185263 +TXonI -6.1932005240380231e-12 7.6891474863570375 +TYonI 7.9326776066916003e-12 -12.208161270489166 +TZonI 0 0 +RevoluteJointSeries /Assembly1/Joint3 +FXonI -9.3476581534717302e-10 88819.123047605608 +FYonI 2.1300711056208707e-09 -139158.77928553589 +FZonI -1.7581627971396568e-11 -91.459335864185164 +TXonI -6.9762750052017134e-11 -137.18900379626888 +TYonI 3.0110848202279314e-11 5.714137793450054e-12 +TZonI -1.692464328348961e-26 -1.9821548728020592e-14 +CylindricalJointSeries /Assembly1/Joint4 +FXonI -9.0072276284998096e-10 88819.123047605593 +FYonI 1.5762188514129319e-25 2.4042424651841576e-11 +FZonI -2.0937008411993682e-11 -91.459335864178428 +TXonI -2.0419967385568454e-11 -45.729667932089207 +TYonI 6.1120231916263287e-26 -6.2889785644848317e-12 +TZonI -9.5778557525619654e-10 88819.123047605695 diff --git a/testapp/draggingBackhoe.log b/testapp/draggingBackhoe.log index 92fa546..b789205 100644 --- a/testapp/draggingBackhoe.log +++ b/testapp/draggingBackhoe.log @@ -179,3 +179,4 @@ runDragStep 1 9.20182797045567e-16 -6.010048927763159e-16 -6.010048927763159e-16 0.91566281475896383 0.40194727224810411 9.20182797045567e-16 -0.40194727224810411 0.91566281475896383 +runPostDrag diff --git a/testapp/draggingBackhoe2.log b/testapp/draggingBackhoe2.log index 12035d0..1c858dc 100644 --- a/testapp/draggingBackhoe2.log +++ b/testapp/draggingBackhoe2.log @@ -17,3 +17,4 @@ runDragStep 1 2.2308684799113093e-15 -8.776108359573272e-16 -1.0922759481826108e-15 0.74987897373328916 0.66157503335049539 2.1339888020027877e-15 -0.66157503335049539 0.74987897373328916 +runPostDrag diff --git a/testapp/draggingBackhoe3.log b/testapp/draggingBackhoe3.log index 3d0c54c..c7a91d8 100644 --- a/testapp/draggingBackhoe3.log +++ b/testapp/draggingBackhoe3.log @@ -26,3 +26,4 @@ runDragStep 1 -4.5522801212393094e-16 -1.9958935382340208e-15 -1.2577026389407931e-15 0.63264657285670811 -0.77444064579066485 1.6152422824418599e-15 0.77444064579066485 0.63264657285670811 +runPostDrag