diff --git a/OndselSolver/ASMTRotationalMotion.cpp b/OndselSolver/ASMTRotationalMotion.cpp index ad01528..b87f9ff 100644 --- a/OndselSolver/ASMTRotationalMotion.cpp +++ b/OndselSolver/ASMTRotationalMotion.cpp @@ -5,7 +5,7 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #include "ASMTRotationalMotion.h" #include "ASMTAssembly.h" #include "SymbolicParser.h" @@ -42,9 +42,15 @@ void MbD::ASMTRotationalMotion::readRotationZ(std::vector& lines) void MbD::ASMTRotationalMotion::initMarkers() { - auto jt = root()->jointAt(motionJoint); - markerI = jt->markerI; - markerJ = jt->markerJ; + if (motionJoint == "") { + assert(markerI != ""); + assert(markerJ != ""); + } + else { + auto jt = root()->jointAt(motionJoint); + markerI = jt->markerI; + markerJ = jt->markerJ; + } } void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) diff --git a/OndselSolver/AccNewtonRaphson.cpp b/OndselSolver/AccNewtonRaphson.cpp index 803768e..98d8ed5 100644 --- a/OndselSolver/AccNewtonRaphson.cpp +++ b/OndselSolver/AccNewtonRaphson.cpp @@ -6,12 +6,15 @@ * See LICENSE file for details about copyright. * ***************************************************************************/ +#include + #include "AccNewtonRaphson.h" #include "SystemSolver.h" #include "Part.h" #include "Constraint.h" #include "SimulationStoppingError.h" -#include +#include "GESpMatParPvPrecise.h" +#include "CREATE.h" using namespace MbD; @@ -120,3 +123,22 @@ void AccNewtonRaphson::preRun() { system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->preAccIC(); }); } + +void MbD::AccNewtonRaphson::handleSingularMatrix() +{ + std::string str = typeid(*matrixSolver).name(); + if (str.find("GESpMatParPvMarkoFast") != std::string::npos) { + matrixSolver = CREATE::With(); + this->solveEquations(); + } + else { + str = typeid(*matrixSolver).name(); + if (str.find("GESpMatParPvPrecise") != std::string::npos) { + this->logSingularMatrixMessage(); + matrixSolver->throwSingularMatrixError("AccAccNewtonRaphson"); + } + else { + assert(false); + } + } +} diff --git a/OndselSolver/AccNewtonRaphson.h b/OndselSolver/AccNewtonRaphson.h index 3427d09..469994d 100644 --- a/OndselSolver/AccNewtonRaphson.h +++ b/OndselSolver/AccNewtonRaphson.h @@ -25,6 +25,7 @@ namespace MbD { void passRootToSystem(); void postRun() override; void preRun() override; + void handleSingularMatrix() override; }; diff --git a/OndselSolver/Array.h b/OndselSolver/Array.h index 821ad7e..7eed1a7 100644 --- a/OndselSolver/Array.h +++ b/OndselSolver/Array.h @@ -43,6 +43,7 @@ namespace MbD { void equalArrayAt(std::shared_ptr> array, int i); virtual void atiput(int i, T value); void magnifySelf(T factor); + void negateSelf(); void atitimes(int i, double factor); virtual std::ostream& printOn(std::ostream& s) const { @@ -172,6 +173,11 @@ namespace MbD { } } template + inline void Array::negateSelf() + { + magnifySelf(-1); + } + template inline void Array::atitimes(int i, double factor) { this->at(i) *= factor; diff --git a/OndselSolver/DiagonalMatrix.h b/OndselSolver/DiagonalMatrix.h index bac3bad..5d9291e 100644 --- a/OndselSolver/DiagonalMatrix.h +++ b/OndselSolver/DiagonalMatrix.h @@ -5,7 +5,7 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "Array.h" @@ -24,10 +24,11 @@ namespace MbD { { // public: + DiagonalMatrix() : Array() {} DiagonalMatrix(int count) : Array(count) {} DiagonalMatrix(int count, const T& value) : Array(count, value) {} DiagonalMatrix(std::initializer_list list) : Array{ list } {} - void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix> diagMat); + void atiputDiagonalMatrix(int i, std::shared_ptr> diagMat); DiagMatsptr times(T factor); FColsptr timesFullColumn(FColsptr fullCol); FMatsptr timesFullMatrix(FMatsptr fullMat); @@ -45,14 +46,6 @@ namespace MbD { std::ostream& printOn(std::ostream& s) const override; }; - template - inline void DiagonalMatrix::atiputDiagonalMatrix(int i, DiagMatsptr diagMat) - { - for (int ii = 0; ii < diagMat->size(); ii++) - { - this->at(i + ii) = diagMat->at(ii); - } - } template<> inline DiagMatDsptr DiagonalMatrix::times(double factor) { @@ -65,6 +58,14 @@ namespace MbD { return answer; } template + inline void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr> diagMat) + { + for (int ii = 0; ii < diagMat->size(); ii++) + { + this->at(i + ii) = diagMat->at(ii); + } + } + template inline DiagMatsptr DiagonalMatrix::times(T factor) { assert(false); diff --git a/OndselSolver/EulerParameters.h b/OndselSolver/EulerParameters.h index 7503040..94ff274 100644 --- a/OndselSolver/EulerParameters.h +++ b/OndselSolver/EulerParameters.h @@ -36,6 +36,7 @@ namespace MbD { this->atiputFullColumn(0, axis->times(sinHalfTheta)); this->atiput(3, cosHalfTheta); this->conditionSelf(); + this->initialize(); this->calc(); } diff --git a/OndselSolver/FullMatrix.h b/OndselSolver/FullMatrix.h index a5899c0..ac2007a 100644 --- a/OndselSolver/FullMatrix.h +++ b/OndselSolver/FullMatrix.h @@ -5,7 +5,7 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "corecrt_math_defines.h" @@ -29,7 +29,7 @@ namespace MbD { class EulerParameters; template class DiagonalMatrix; - + using FMatFColDsptr = std::shared_ptr>; using FMatFMatDsptr = std::shared_ptr>; using FColFMatDsptr = std::shared_ptr>; @@ -102,6 +102,7 @@ namespace MbD { double maxMagnitude() override; FColsptr bryantAngles(); bool isDiagonal(); + bool isDiagonalToWithin(double ratio); std::shared_ptr> asDiagonalMatrix(); void conditionSelfWithTol(double tol); @@ -332,13 +333,13 @@ namespace MbD { tilde->atijput(2, 1, c0); return tilde; } - template<> - inline void FullMatrix::zeroSelf() - { - for (int i = 0; i < this->size(); i++) { - this->at(i)->zeroSelf(); - } - } + template<> + inline void FullMatrix::zeroSelf() + { + for (int i = 0; i < this->size(); i++) { + this->at(i)->zeroSelf(); + } + } template<> inline void FullMatrix::identity() { this->zeroSelf(); @@ -680,6 +681,27 @@ namespace MbD { return true; } template + inline bool FullMatrix::isDiagonalToWithin(double ratio) + { + auto maxMag = this->maxMagnitude(); + auto tol = ratio * maxMag; + auto nrow = this->nrow(); + if (nrow == this->ncol()) { + for (int i = 0; i < 3; i++) + { + for (int j = i + 1; j < 3; j++) + { + if (std::abs(this->at(i)->at(j)) > tol) return false; + if (std::abs(this->at(j)->at(i)) > tol) return false; + } + } + return true; + } + else { + return false; + } + } + template inline std::shared_ptr> FullMatrix::asDiagonalMatrix() { int nrow = this->nrow(); diff --git a/OndselSolver/GESpMatParPvPrecise.cpp b/OndselSolver/GESpMatParPvPrecise.cpp index ed4ac2a..5ee7cc1 100644 --- a/OndselSolver/GESpMatParPvPrecise.cpp +++ b/OndselSolver/GESpMatParPvPrecise.cpp @@ -29,7 +29,7 @@ void GESpMatParPvPrecise::doPivoting(int p) while (lookForFirstNonZeroInPivotCol) { spRowi = matrixA->at(i); if (spRowi->find(p) == spRowi->end()) { - if (i <= p) throwSingularMatrixError(""); + if (i <= p) throwSingularMatrixError("doPivoting"); } else { markowitzPivotColCount = 0; @@ -71,7 +71,7 @@ void GESpMatParPvPrecise::doPivoting(int p) if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; } pivotValues->at(p) = max; - if (max < singularPivotTolerance) throwSingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError("doPivoting"); } void GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) @@ -97,7 +97,7 @@ void GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr f { auto& spRowi = spMat->at(i); auto maxRowMagnitude = spRowi->maxMagnitude(); - if (maxRowMagnitude == 0) throwSingularMatrixError(""); + if (maxRowMagnitude == 0) throwSingularMatrixError("preSolvewithsaveOriginal"); rowScalings->at(i) = 1.0 / maxRowMagnitude; matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude); rowOrder->at(i) = i; diff --git a/OndselSolver/MBDynBody.cpp b/OndselSolver/MBDynBody.cpp index 26e48ef..6f56cfd 100644 --- a/OndselSolver/MBDynBody.cpp +++ b/OndselSolver/MBDynBody.cpp @@ -5,6 +5,7 @@ #include "BasicUserFunction.h" #include "ASMTPart.h" #include "ASMTAssembly.h" +#include "MomentOfInertiaSolver.h" using namespace MbD; @@ -85,10 +86,25 @@ void MbD::MBDynBody::createASMT() auto asmtMassMarker = std::make_shared(); asmtItem = asmtMassMarker; asmtMassMarker->setMass(mass); - assert(aJmat->isDiagonal()); - asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix()); - asmtMassMarker->setPosition3D(rPcmP); - asmtMassMarker->setRotationMatrix(FullMatrix::identitysptr(3)); - auto asmtPart = asmtAssembly()->partPartialNamed(nodeName); - asmtPart->setPrincipalMassMarker(asmtMassMarker); + if (aJmat->isDiagonalToWithin(1.0e-6)) { + asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix()); + asmtMassMarker->setPosition3D(rPcmP); + asmtMassMarker->setRotationMatrix(FullMatrix::identitysptr(3)); + auto asmtPart = asmtAssembly()->partPartialNamed(nodeName); + asmtPart->setPrincipalMassMarker(asmtMassMarker); + } + else { + auto solver = std::make_shared(); + solver->setm(mass); + solver->setJPP(aJmat); + solver->setrPoP(rPcmP); + solver->setAPo(FullMatrix::identitysptr(3)); + solver->setrPcmP(rPcmP); + solver->calc(); + asmtMassMarker->setMomentOfInertias(solver->aJpp); + asmtMassMarker->setPosition3D(rPcmP); + asmtMassMarker->setRotationMatrix(solver->aAPp); + auto asmtPart = asmtAssembly()->partPartialNamed(nodeName); + asmtPart->setPrincipalMassMarker(asmtMassMarker); + } } diff --git a/OndselSolver/MBDynCase.mbd b/OndselSolver/MBDynCase.mbd index f3bb82e..a4ac3da 100644 --- a/OndselSolver/MBDynCase.mbd +++ b/OndselSolver/MBDynCase.mbd @@ -23,7 +23,7 @@ begin: initial value; initial time: 0.0; final time: 8.0; - time step: 0.01; + time step: 0.1; max iterations: 100; tolerance: 1e-06; derivatives tolerance: 0.0001; diff --git a/OndselSolver/MBDynCase.mov b/OndselSolver/MBDynCase.mov new file mode 100644 index 0000000..4766ac4 --- /dev/null +++ b/OndselSolver/MBDynCase.mov @@ -0,0 +1,324 @@ +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749178 -0.09687939551894122 -3.252606517456513e-19 0.9999846242181696 -0.005545387925678226 0.09688088513829464 0.005519302327492587 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051885 0.9952948074630237 -0.001621407113303408 -0.09687939551894119 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.09688683977859862 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981437 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 +1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 +2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 +3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 +4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 diff --git a/OndselSolver/MBDynJoint.cpp b/OndselSolver/MBDynJoint.cpp index be29cf7..997bf7b 100644 --- a/OndselSolver/MBDynJoint.cpp +++ b/OndselSolver/MBDynJoint.cpp @@ -9,6 +9,7 @@ #include "ASMTPointInLineJoint.h" #include "ASMTNoRotationJoint.h" #include "ASMTFixedJoint.h" +#include "ASMTSphericalJoint.h" using namespace MbD; @@ -219,9 +220,23 @@ void MbD::MBDynJoint::createASMT() asmtAsm->addMotion(asmtMotion); return; } + if (joint_type == "drive hinge") { + auto asmtAsm = asmtAssembly(); + auto asmtMotion = std::make_shared(); + asmtItem = asmtMotion; + asmtMotion->setName(name); + asmtMotion->setMarkerI(mkr1->asmtItem->fullName("")); + asmtMotion->setMarkerJ(mkr2->asmtItem->fullName("")); + asmtMotion->setRotationZ(formula); + asmtAsm->addMotion(asmtMotion); + return; + } if (joint_type == "revolute hinge") { asmtJoint = std::make_shared(); } + else if (joint_type == "spherical hinge") { + asmtJoint = std::make_shared(); + } else if (joint_type == "in line") { asmtJoint = std::make_shared(); } diff --git a/OndselSolver/MomentOfInertiaSolver.cpp b/OndselSolver/MomentOfInertiaSolver.cpp index 81abade..5451c2e 100644 --- a/OndselSolver/MomentOfInertiaSolver.cpp +++ b/OndselSolver/MomentOfInertiaSolver.cpp @@ -1,6 +1,7 @@ #include "MomentOfInertiaSolver.h" #include "EulerParameters.h" #include +#include using namespace MbD; @@ -13,8 +14,8 @@ void MbD::MomentOfInertiaSolver::example1() }); auto rpPp = std::make_shared>(ListD{ 0, 0, 1 }); - auto axis = std::make_shared>(ListD{ 0, 0, 1 }); - auto aApP = std::make_shared>(axis, 0.1)->aA; + auto rotAxis = std::make_shared>(ListD{ 0, 0, 1 }); + auto aApP = std::make_shared>(rotAxis, M_PI*10/180)->aA; auto solver = std::make_shared(); solver->setm(4.0); solver->setJPP(aJpp); @@ -24,10 +25,9 @@ void MbD::MomentOfInertiaSolver::example1() solver->setrPcmP(rPcmP); solver->calc(); auto aJPP = solver->getJoo(); - std::cout << solver->getJoo(); - std::cout << solver->getJpp(); - std::cout << solver->getAPp(); - std::cout << std::endl; + std::cout << *solver->getJoo() << std::endl; + std::cout << *solver->getJpp() << std::endl; + std::cout << *solver->getAPp() << std::endl; solver->setm(4.0); solver->setJPP(aJPP); auto rPoP = aApP->transposeTimesFullColumn(rpPp->negated()); @@ -36,14 +36,59 @@ void MbD::MomentOfInertiaSolver::example1() solver->setAPo(aAPo); solver->setrPcmP(rPoP); solver->calc(); - std::cout << solver->getJoo(); - std::cout << solver->getJpp(); - std::cout << solver->getAPp(); - std::cout << std::endl; + std::cout << *solver->getJoo() << std::endl; + std::cout << *solver->getJpp() << std::endl; + std::cout << *solver->getAPp() << std::endl; +} + +void MbD::MomentOfInertiaSolver::doFullPivoting(int p) +{ + auto max = 0.0; + auto pivotRow = p; + auto pivotCol = p; + for (int i = p; i < 3; i++) + { + auto rowi = aJcmPcopy->at(i); + for (int j = p; j < 3; j++) + { + auto aij = rowi->at(j); + if (aij != 0.0) { + auto mag = std::abs(aij); + if (mag > max) { + max = mag; + pivotRow = i; + pivotCol = j; + } + } + } + + } + if (p != pivotRow) aJcmPcopy->swapElems(p, pivotRow); + if (p != pivotCol) { + for (auto& row : *aJcmPcopy) { + row->swapElems(p, pivotCol); + } + colOrder->swapElems(p, pivotCol); + } } void MbD::MomentOfInertiaSolver::forwardEliminateWithPivot(int p) { + auto rowp = aJcmPcopy->at(p); + auto app = rowp->at(p); + for (int i = p + 1; i < 3; i++) + { + auto rowi = aJcmPcopy->at(i); + auto aip = rowi->at(p); + if (aip != 0) { + rowi->atiput(p, 0.0); + auto factor = aip / app; + for (int j = p + 1; j < 3; j++) + { + rowi->atiminusNumber(j, factor * rowp->at(j)); + } + } + } } void MbD::MomentOfInertiaSolver::backSubstituteIntoDU() @@ -111,7 +156,7 @@ FMatDsptr MbD::MomentOfInertiaSolver::getJoo() return aJoo; } -FMatDsptr MbD::MomentOfInertiaSolver::getJpp() +DiagMatDsptr MbD::MomentOfInertiaSolver::getJpp() { return aJpp; } @@ -150,8 +195,207 @@ void MbD::MomentOfInertiaSolver::calcJoo() void MbD::MomentOfInertiaSolver::calcJpp() { + //"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)" + + auto rPcmPtilde = FullMatrix::tildeMatrix(rPcmP); + aJcmP = aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m)); + aJcmP->symLowerWithUpper(); + aJcmP->conditionSelfWithTol(aJcmP->maxMagnitude() * 1.0e-6); + if (aJcmP->isDiagonal()) { + calcJppFromDiagJcmP(); + } + else { + calcJppFromFullJcmP(); + } } void MbD::MomentOfInertiaSolver::calcAPp() { + FColDsptr eigenvector1, eigenvector2, eigenvector0; + auto lam0 = aJpp->at(0); + auto lam1 = aJpp->at(1); + auto lam2 = aJpp->at(2); + if (lam0 == lam1) { + if (lam1 == lam2) { + aAPp = FullMatrix::identitysptr(3); + } + else { + eigenvector1 = eigenvectorFor(lam1); + eigenvector2 = eigenvectorFor(lam2); + eigenvector1->normalizeSelf(); + eigenvector2->normalizeSelf(); + if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf(); + if (eigenvector2->at(2) < 0.0) eigenvector2->negateSelf(); + eigenvector0 = eigenvector1->cross(eigenvector2); + } + } + else { + eigenvector0 = eigenvectorFor(lam0); + eigenvector1 = eigenvectorFor(lam1); + eigenvector0->normalizeSelf(); + eigenvector1->normalizeSelf(); + if (eigenvector0->at(0) < 0.0) eigenvector0->negateSelf(); + if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf(); + eigenvector2 = eigenvector0->cross(eigenvector1); + } + aAPp = std::make_shared>(3, 3); + aAPp->atijputFullColumn(0, 0, eigenvector0); + aAPp->atijputFullColumn(0, 1, eigenvector1); + aAPp->atijputFullColumn(0, 2, eigenvector2); +} + +FColDsptr MbD::MomentOfInertiaSolver::eigenvectorFor(double lam) +{ + //"[aJcmP] - lam[I]." + + double e0, e1, e2; + aJcmPcopy = aJcmP->copy(); + colOrder = std::make_shared>(3); + auto eigenvector = std::make_shared>(3); + for (int i = 0; i < 3; i++) + { + colOrder->atiput(i, i); + aJcmPcopy->atijminusNumber(i, i, lam); + } + for (int p = 0; p < 3; p++) + { + doFullPivoting(p); + forwardEliminateWithPivot(p); + } + + auto row0 = aJcmPcopy->at(0); + auto row1 = aJcmPcopy->at(1); + auto row2 = aJcmPcopy->at(2); + auto norm0 = row0->length(); + auto aaaa = row2->length(); + if ((row2->length() / norm0) > 1.0e-5) throw std::runtime_error("3rd row should be very small."); + if ((row1->length() / norm0) > 1.0e-5) { + e2 = 1.0; + e1 = e2 * -row1->at(2) / row1->at(1); + e0 = -(e1 * row0->at(1) + e2 * row0->at(2)) / row0->at(0); + } + else { + //"Repeated roots." + e2 = 1.0; + e1 = 0.0; + e0 = -(e1 * row0->at(1) + e2 * row0->at(2)) / row0->at(0); + } + + + auto dum = std::make_shared>(ListD{ e0, e1, e2 }); + for (int i = 0; i < 3; i++) + { + eigenvector->atiput(colOrder->at(i), dum->at(i)); + } + return eigenvector; +} + +void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP() +{ + //"Eigenvalues are orders from smallest to largest." + + double average; + auto sortedJ = std::make_shared>(); + sortedJ->push_back(aJcmP->at(0)->at(0)); + sortedJ->push_back(aJcmP->at(1)->at(1)); + sortedJ->push_back(aJcmP->at(2)->at(2)); + std::sort(sortedJ->begin(), sortedJ->end(), [](double a, double b) { return a < b; }); + auto lam0 = sortedJ->at(0); + auto lam1 = sortedJ->at(1); + auto lam2 = sortedJ->at(2); + if (lam1 == 0 || std::abs((lam0 / lam1) - 1.0) < 1.0e-6) { + if (lam2 == 0 || std::abs((lam1 / lam2) - 1.0) < 1.0e-6) { + //"All are equal." + average = (lam0 + lam1 + lam2) / 3.0; + lam0 = average; + lam1 = average; + lam2 = average; + } + else { + //"lam0 = lam1" + average = (lam0 + lam1) / 2.0; + lam0 = average; + lam1 = average; + } + } + else { + if (std::abs(lam1 / lam2 - 1.0) < 1.0e-6) { + //"lam1 = lam2" + average = (lam1 + lam2) / 2.0; + lam1 = average; + lam2 = average; + } + } + aJpp = std::make_shared>(ListD{ lam0, lam1, lam2 }); +} + +void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP() +{ + //"Eigenvalues are orders from smallest to largest." + //"Rounding error can be significant." + //"e.g. (diag 100.0d 100.0d 1.0d) gives only 8 digit accuracy." + + double average; + auto a00 = aJcmP->at(0)->at(0); + auto a01 = aJcmP->at(0)->at(1); + auto a02 = aJcmP->at(0)->at(2); + auto a11 = aJcmP->at(1)->at(1); + auto a12 = aJcmP->at(1)->at(2); + auto a22 = aJcmP->at(2)->at(2); + auto a = -(a00 + a11 + a22); + auto b = a00 * a11 + (a00 * a22) + (a11 * a22) - (a01 * a01) - (a02 * a02) - (a12 * a12); + auto c = a00 * a12 * a12 + (a11 * a02 * a02) + (a22 * a01 * a01) - (a00 * a11 * a22) - (2.0 * a01 * a02 * a12); + auto aDiv3 = a / 3.0; + auto p = (b / 3.0) - (aDiv3 * aDiv3); + auto q = (c + (2.0 * aDiv3 * aDiv3 * aDiv3) - (aDiv3 * b)) / 2.0; + auto phiDiv3 = modifiedArcCos(-q / std::sqrt(-p * p * p)) / 3.0; + auto twoSqrtMinusp = 2.0 * std::sqrt(-p); + auto piDiv3 = M_PI / 3.0; + auto sortedJ = std::make_shared>(); + sortedJ->push_back(twoSqrtMinusp * std::cos(phiDiv3)); + sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 + piDiv3)); + sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 - piDiv3)); + std::sort(sortedJ->begin(), sortedJ->end(), [](double a, double b) { return a < b; }); + auto lam0 = sortedJ->at(0) - aDiv3; + auto lam1 = sortedJ->at(1) - aDiv3; + auto lam2 = sortedJ->at(2) - aDiv3; + if (lam1 == 0 || std::abs((lam0 / lam1) - 1.0) < 1.0e-6) { + if (lam2 == 0 || std::abs((lam1 / lam2) - 1.0) < 1.0e-6) { + //"All are equal." + average = (lam0 + lam1 + lam2) / 3.0; + lam0 = average; + lam1 = average; + lam2 = average; + } + else { + //"lam0 = lam1" + average = (lam0 + lam1) / 2.0; + lam0 = average; + lam1 = average; + } + } + else { + if (std::abs((lam1 / lam2) - 1.0) < 1.0e-6) { + //"lam1 = lam2" + average = (lam1 + lam2) / 2.0; + lam1 = average; + lam2 = average; + } + } + aJpp = std::make_shared>(ListD{ lam0, lam1, lam2 }); +} + +double MbD::MomentOfInertiaSolver::modifiedArcCos(double val) +{ + if (val > 1.0) { + return 0.0; + } + else { + if (val < -1.0) { + return M_PI; + } + else { + return std::acos(val); + } + } } diff --git a/OndselSolver/MomentOfInertiaSolver.h b/OndselSolver/MomentOfInertiaSolver.h index a08f39e..4c80767 100644 --- a/OndselSolver/MomentOfInertiaSolver.h +++ b/OndselSolver/MomentOfInertiaSolver.h @@ -18,6 +18,7 @@ namespace MbD { //aJoo == aJpp when rPoP == rPcmP and aAPo == aAPp public: static void example1(); + void doFullPivoting(int p); void forwardEliminateWithPivot(int p) override; void backSubstituteIntoDU() override; void postSolve() override; @@ -34,16 +35,22 @@ namespace MbD { void setAPo(FMatDsptr mat); void setrPcmP(FColDsptr col); FMatDsptr getJoo(); - FMatDsptr getJpp(); + DiagMatDsptr getJpp(); FMatDsptr getAPp(); void calc(); void calcJoo(); void calcJpp(); void calcAPp(); + FColDsptr eigenvectorFor(double lam); + void calcJppFromDiagJcmP(); + void calcJppFromFullJcmP(); + double modifiedArcCos(double val); + double m; - FMatDsptr aJPP, aJoo, aAPo, aJcmP, aJcmPcopy, aJpp, aAPp; + FMatDsptr aJPP, aJoo, aAPo, aJcmP, aJcmPcopy, aAPp; FColDsptr rPoP, rPcmP; + DiagMatDsptr aJpp; std::shared_ptr> colOrder; }; diff --git a/OndselSolver/OndselSolver.cpp b/OndselSolver/OndselSolver.cpp index b0da8b5..080d95c 100644 --- a/OndselSolver/OndselSolver.cpp +++ b/OndselSolver/OndselSolver.cpp @@ -18,18 +18,18 @@ #include "GESpMatParPvPrecise.h" #include "ASMTAssembly.h" #include "MBDynSystem.h" +#include "MomentOfInertiaSolver.h" using namespace MbD; void runSpMat(); int main() { - - //MBDynSystem::runFile("MBDynCase.mbd"); //To be completed + MBDynSystem::runFile("MBDynCase.mbd"); //To be completed MBDynSystem::runFile("crank_slider.mbd"); //To be completed //ASMTAssembly::runSinglePendulumSuperSimplified(); //Mass is missing //ASMTAssembly::runSinglePendulumSuperSimplified2(); //DOF has infinite acceleration due to zero mass and inertias - //ASMTAssembly::runSinglePendulumSimplified(); + ASMTAssembly::runSinglePendulumSimplified(); ASMTAssembly::runSinglePendulum(); ASMTAssembly::runFile("piston.asmt"); ASMTAssembly::runFile("00backhoe.asmt"); @@ -46,6 +46,7 @@ int main() //cadSystem->runOndselPiston(); //For debugging cadSystem->runPiston(); runSpMat(); + MomentOfInertiaSolver::example1(); } void runSpMat() { diff --git a/OndselSolver/Solver.cpp b/OndselSolver/Solver.cpp index 4b966bd..20830f1 100644 --- a/OndselSolver/Solver.cpp +++ b/OndselSolver/Solver.cpp @@ -69,3 +69,8 @@ void Solver::logString(const char* chars) std::string str = chars; this->logString(str); } + +void MbD::Solver::handleSingularMatrix() +{ + assert(false); +} diff --git a/OndselSolver/Solver.h b/OndselSolver/Solver.h index 3e252b4..4c5247d 100644 --- a/OndselSolver/Solver.h +++ b/OndselSolver/Solver.h @@ -28,6 +28,7 @@ namespace MbD { virtual void logString(std::string& str); void logString(const char* chars); virtual void setSystem(Solver* sys) = 0; + virtual void handleSingularMatrix(); }; } diff --git a/OndselSolver/VectorNewtonRaphson.cpp b/OndselSolver/VectorNewtonRaphson.cpp index 33c71a8..82b51c8 100644 --- a/OndselSolver/VectorNewtonRaphson.cpp +++ b/OndselSolver/VectorNewtonRaphson.cpp @@ -77,8 +77,3 @@ void VectorNewtonRaphson::xEqualxoldPlusdx() { x = xold->plusFullColumn(dx); } - -void VectorNewtonRaphson::handleSingularMatrix() -{ - assert(false); -} diff --git a/OndselSolver/VectorNewtonRaphson.h b/OndselSolver/VectorNewtonRaphson.h index ff63474..ee35d43 100644 --- a/OndselSolver/VectorNewtonRaphson.h +++ b/OndselSolver/VectorNewtonRaphson.h @@ -27,7 +27,6 @@ namespace MbD { bool isConverged() override; void xEqualxoldPlusdx() override; virtual void basicSolveEquations() = 0; - virtual void handleSingularMatrix(); std::shared_ptr matrixSolver; int n; diff --git a/OndselSolver/VelSolver.h b/OndselSolver/VelSolver.h index aa1b4d7..7c15d1b 100644 --- a/OndselSolver/VelSolver.h +++ b/OndselSolver/VelSolver.h @@ -23,7 +23,7 @@ namespace MbD { //system n x errorVector jacobian matrixSolver public: void basicSolveEquations(); - void handleSingularMatrix(); + void handleSingularMatrix() override; void logSingularMatrixMessage(); std::shared_ptr matrixSolverClassNew(); void solveEquations();