Ready for merge.

Ondsel-Development · Nov 29, 2023 · 4ba630e · 4ba630e
1 parent 5dd49cb
commit 4ba630e
Show file tree

Hide file tree

Showing 13 changed files with 557 additions and 567 deletions.
diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp
@@ -55,6 +55,7 @@
 #include "ASMTPerpendicularJoint.h"
 #include "ASMTRackPinionJoint.h"
 #include "ASMTScrewJoint.h"
+#include "SimulationStoppingError.h"
 
 using namespace MbD;
 
@@ -1038,7 +1039,12 @@ void MbD::ASMTAssembly::runKINEMATIC()
 	auto mbdSystem = std::make_shared<System>();
 	mbdObject = mbdSystem;
 	mbdSystem->externalSystem->asmtAssembly = this;
-	mbdSystem->runKINEMATIC(mbdSystem);
+	try {
+		mbdSystem->runKINEMATIC(mbdSystem);
+	}
+	catch (SimulationStoppingError ex) {
+
+	}
 }
 
 void MbD::ASMTAssembly::initprincipalMassMarker()

diff --git a/OndselSolver/ASMTSpatialContainer.cpp b/OndselSolver/ASMTSpatialContainer.cpp
@@ -333,6 +333,7 @@ void MbD::ASMTSpatialContainer::updateFromMbD()
 	auto mbdPart = std::static_pointer_cast<Part>(mbdObject);
 	auto rOcmO = mbdPart->qX()->times(mbdUnts->length);
 	auto aAOp = mbdPart->aAOp();
+	std::cout << "aAOp" << *aAOp << std::endl;
 	auto vOcmO = mbdPart->qXdot()->times(mbdUnts->velocity);
 	auto omeOPO = mbdPart->omeOpO()->times(mbdUnts->omega);
 	omega3D = omeOPO;
@@ -355,6 +356,7 @@ void MbD::ASMTSpatialContainer::updateFromMbD()
 	bryxs->push_back(bryantAngles->at(0));
 	bryys->push_back(bryantAngles->at(1));
 	bryzs->push_back(bryantAngles->at(2));
+	std::cout << "bry " << *bryantAngles << std::endl;
 	vxs->push_back(vOPO->at(0));
 	vys->push_back(vOPO->at(1));
 	vzs->push_back(vOPO->at(2));

diff --git a/OndselSolver/AngleZIecJec.cpp b/OndselSolver/AngleZIecJec.cpp
@@ -29,7 +29,7 @@ void MbD::AngleZIecJec::calcPostDynCorrectorIteration()
 	auto diffOfSquares = sthez * sthez - (cthez * cthez);
 	auto sumOfSquaresSquared = sumOfSquares * sumOfSquares;
 	auto thez0to2pi = Numeric::arcTan0to2piYoverX(sthez, cthez);
-	thez = std::round(thez - thez0to2pi / (2.0 * M_PI)) * (2.0 * M_PI) + thez0to2pi;
+	thez = std::round(thez - thez0to2pi / (2.0 * OS_M_PI)) * (2.0 * OS_M_PI) + thez0to2pi;
 	cosOverSSq = cthez / sumOfSquares;
 	sinOverSSq = sthez / sumOfSquares;
 	twoCosSinOverSSqSq = 2.0 * cthez * sthez / sumOfSquaresSquared;

diff --git a/OndselSolver/FullColumn.h b/OndselSolver/FullColumn.h
@@ -47,8 +47,9 @@ namespace MbD {
 		void equalSelfPlusFullColumntimes(FColsptr<T> fullCol, T factor);
 		FColsptr<T> cross(FColsptr<T> fullCol);
 		FColsptr<T> simplified();
-		std::shared_ptr<FullColumn<T>> clonesptr();
 		double dot(std::shared_ptr<FullVector<T>> vec);
+		std::shared_ptr<FullColumn<T>> cloneFcSptr();
+		double dotVec(std::shared_ptr<FullVector<T>> vec);
 		std::shared_ptr<FullVector<T>> dot(std::shared_ptr<std::vector<std::shared_ptr<FullColumn<T>>>> vecvec);
 
 		std::ostream& printOn(std::ostream& s) const override;

diff --git a/OndselSolver/MomentOfInertiaSolver.cpp b/OndselSolver/MomentOfInertiaSolver.cpp
@@ -15,7 +15,7 @@ void MbD::MomentOfInertiaSolver::example1()
 
 	auto rpPp = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
 	auto rotAxis = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
-	auto aApP = std::make_shared<EulerParameters<double>>(rotAxis, M_PI*10/180)->aA;
+	auto aApP = std::make_shared<EulerParameters<double>>(rotAxis, OS_M_PI*10/180)->aA;
 	auto solver = std::make_shared<MomentOfInertiaSolver>();
 	solver->setm(4.0);
 	solver->setJPP(aJpp);

diff --git a/OndselSolver/Numeric.cpp b/OndselSolver/Numeric.cpp
@@ -26,22 +26,22 @@ double MbD::Numeric::arcTan0to2piYoverX(double y, double x)
 		}
 		else {
 			//"Forth quadrant."
-			return 2.0 * M_PI + std::atan2(y, x);
+			return 2.0 * OS_M_PI + std::atan2(y, x);
 		}
 	}
 	else {
 		if (x < 0.0) {
 			//"Second and Third quadrants."
-			return	M_PI + std::atan2(y, x);
+			return	OS_M_PI + std::atan2(y, x);
 		}
 		else {
 			//"x = 0"
 			if (y > 0.0) {
-				return M_PI / 2.0;
+				return OS_M_PI / 2.0;
 			}
 			else {
 				if (y < 0.0) {
-					return 1.5 * M_PI;
+					return 1.5 * OS_M_PI;
 				}
 				else {
 					throw std::invalid_argument("atan2(0, 0) is not defined.");

diff --git a/OndselSolver/OrbitAngleZIecJec.cpp b/OndselSolver/OrbitAngleZIecJec.cpp
@@ -29,9 +29,9 @@ void MbD::OrbitAngleZIecJec::calcPostDynCorrectorIteration()
 	auto diffOfSquares = y * y - (x * x);
 	auto sumOfSquaresSquared = sumOfSquares * sumOfSquares;
 	auto thez0to2pi = Numeric::arcTan0to2piYoverX(y, x);
-	thez = std::round((thez - thez0to2pi) / (2.0 * M_PI)) * (2.0 * M_PI) + thez0to2pi;
-	auto cosOverSSq = x / sumOfSquares;
-	auto sinOverSSq = y / sumOfSquares;
+	thez = std::round((thez - thez0to2pi) / (2.0 * OS_M_PI)) * (2.0 * OS_M_PI) + thez0to2pi;
+	cosOverSSq = x / sumOfSquares;
+	sinOverSSq = y / sumOfSquares;
 	twoCosSinOverSSqSq = 2.0 * x * y / sumOfSquaresSquared;
 	dSqOverSSqSq = diffOfSquares / sumOfSquaresSquared;
 }

diff --git a/OndselSolver/ScrewConstraintIJ.cpp b/OndselSolver/ScrewConstraintIJ.cpp
@@ -66,7 +66,7 @@ void MbD::ScrewConstraintIJ::postInput()
 {
 	zIeJeIe->postInput();
 	thezIeJe->postInput();
-	aConstant = (2.0 * M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch);
+	aConstant = (2.0 * OS_M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch);
 	ConstraintIJ::postInput();
 }
 

diff --git a/OndselSolver/ScrewConstraintIqcJc.cpp b/OndselSolver/ScrewConstraintIqcJc.cpp
@@ -64,18 +64,18 @@ void MbD::ScrewConstraintIqcJc::calc_pGpEI()
 
 void MbD::ScrewConstraintIqcJc::calc_pGpXI()
 {
-	pGpXI = zIeJeIe->pvaluepXI()->times(2.0 * M_PI);
+	pGpXI = zIeJeIe->pvaluepXI()->times(2.0 * OS_M_PI);
 }
 
 void MbD::ScrewConstraintIqcJc::calc_ppGpEIpEI()
 {
-	ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)
+	ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * OS_M_PI)
 		->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch));
 }
 
 void MbD::ScrewConstraintIqcJc::calc_ppGpXIpEI()
 {
-	ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)
+	ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * OS_M_PI)
             ->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch));
 }
 

diff --git a/OndselSolver/ScrewConstraintIqcJqc.cpp b/OndselSolver/ScrewConstraintIqcJqc.cpp
@@ -42,23 +42,23 @@ void MbD::ScrewConstraintIqcJqc::calc_pGpEJ()
 
 void MbD::ScrewConstraintIqcJqc::calc_pGpXJ()
 {
-	pGpXJ = zIeJeIe->pvaluepXJ()->times(2.0 * M_PI);
+	pGpXJ = zIeJeIe->pvaluepXJ()->times(2.0 * OS_M_PI);
 }
 
 void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpEJ()
 {
-	ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)
+	ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * OS_M_PI)
 		->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch));
 }
 
 void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpXJ()
 {
-	ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI);
+	ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * OS_M_PI);
 }
 
 void MbD::ScrewConstraintIqcJqc::calc_ppGpEJpEJ()
 {
-	ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)
+	ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * OS_M_PI)
             ->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch));
 }
 

diff --git a/testapp/MBDynCase9orig.mov b/testapp/MBDynCase9orig.mov
@@ -853,22 +853,3 @@
 17 0.03907519316942942 0.02184863773088035 0.004371177800044147 0.3749534842899686 0.5803823387449342 0.7228874224191294 0.7999686528251818 0.1914821164730324 -0.5686692831233913 -0.4684656221281088 0.7915118065726932 -0.3924907908985322 0.0007100630677400266 7.021996864484864e-05 0.002027676134871538 -0.03425244253385289 0.1782345857287144 0.005822314125438779 
 18 0.007947959887565109 0.02501638734887722 0.145904272692078 -0.5493678696610178 0.7057635513256313 -0.4473172849380491 -0.8025694539401015 -0.2966991674135582 0.5175440808843133 0.2325450824816602 0.6433252782817835 0.7294212575304229 -0.01729655824609937 0.001653467922446419 -0.007794083269734505 0.427549016233553 -0.3119965922711967 -1.129420581918531 
 19 0.003020293326525449 0.02054000227593566 -0.09962522199980411 -0.7567706543278723 0.5401726677867117 0.3681190917683444 -0.6521466652885279 -0.5853388676302408 -0.4817500773164443 -0.04475381214172292 -0.604641959359036 0.7952390818371851 -0.02716507862770892 -0.001307437026432216 0.01291552941301775 -0.8847769914256864 0.4356586649410015 -1.933643994606818 
-1 0.01503650608232607 0.02000685600832166 0.02190382736368595 1 0 0 0 1 0 0 0 1 -2.805373121306243e-18 -7.616520261710669e-18 4.77048955893622e-18 -1.110223024625157e-16 0 1.068214387740352e-16 
-2 0.02003650608232608 0.02200685600832166 0.03190382736368595 6.123233995736766e-17 0 1 -0.982292479339031 0.187353903172524 6.014806703245309e-17 -0.187353903172524 -0.982292479339031 1.147211789139974e-17 -5.999518462531188e-18 1.094020309277243e-05 -6.096139600113455e-06 -0.8741627312177636 3.05311331771918e-16 -2.498001805406602e-16 
-3 0.02003650608232607 0.02200685600832166 0.01190382736368595 6.123233995736766e-17 0 1 -0.9822924793390309 -0.1873539031725241 6.014806703245309e-17 0.1873539031725241 -0.9822924793390309 -1.147211789139974e-17 6.328939715967466e-19 1.038829567894781e-05 6.995352210517489e-06 0.8741627312177633 -1.387778780781446e-16 2.775557561562891e-16 
-4 0.02884222035448263 0.02045036646573431 0.02976088893018847 -0.1954552357823489 -0.04630087015290779 0.9796190485226132 0.9164696766363609 -0.3642062191643265 0.1656416666423961 0.349113996584655 0.9301666836353097 0.1136193559371788 4.935030726083922e-06 -0.001987700502615454 0.001192184379051397 0.0274220107176535 0.142695101994344 0.1177417737977026 
-5 0.03017589000659163 0.02114176531143216 0.03045608163383956 -0.9706888907943509 -0.1419501445148493 -0.1939413152493454 -0.01320444356812887 -0.7742303071053569 0.6327662081919361 -0.239976498762876 0.6167800159147165 0.7496623853507346 2.661641932565371e-05 -0.00185472780037388 0.001029131230760915 0.0676797915865314 0.01279194527502274 -0.03365706938609245 
-6 -8.00152482792514e+23 -1.426390452847778e+23 8.105200993060337e+22 6.123233995736766e-17 0 1 -0.2619713644504894 0.9650756468836776 1.604111964712783e-17 -0.9650756468836776 -0.2619713644504894 5.909384009455786e-17 -4.971788559324274e+35 3.224133649017215e+36 7.65783642579937e+35 1788932244990.865 1275950716535.232 -4210610116847.852 
-7 -2.490523773729768e+32 -1.528329954874561e+32 1.101240703753665e+33 6.123233995736766e-17 0 -1 -0.6957769951477608 0.7182578736241968 -4.260405350140344e-17 0.7182578736241968 0.6957769951477608 4.398061029481284e-17 6.057549326199568e+48 5.978289191376748e+49 9.713643289569628e+48 -5.2627619671313e+16 6528620956450868 -7361340824898506 
-8 -3.107543146503156e+18 -5.539199266513592e+17 3.149169039665571e+17 6.123233995736766e-17 0 1 -0.7176275801145556 0.6964270645652185 4.394201594835757e-17 -0.6964270645652185 -0.7176275801145556 4.26438587729691e-17 -4.731562396203353e+27 2.506003088927895e+28 -2.611113643461041e+27 -1545353742.489743 -1115709649.615911 -7907652996.166046 
-9 0.05529193466459995 0.02492269146501569 0.03282861154052663 -0.1852571080403268 -0.07762939965588994 0.9796190485232517 0.9636787220254812 -0.20949023602367 0.1656416666363945 0.1923619625494347 0.9747243288847434 0.1136193559404239 4.35184917014701e-06 0.001030495541929467 -0.002469782268665737 -0.06674874949863671 0.1279791206893227 0.1053004130202901 
-10 0.05663460802123519 0.02585230170224983 0.03309889484414008 -0.8874247346547703 -0.4238959535821187 -0.1810788802141676 -0.0991488323759573 -0.2081052492155571 0.9730681961134149 -0.4501631363865715 0.8814785452781237 0.1426489567188205 -6.466119731390245e-05 0.001185523505537329 -0.002706067781790921 -0.04943046923425913 0.03050990731902693 0.09058737411683973 
-11 -1.664530274066668e+22 -1.779640398882054e+22 -6.149944033226566e+21 6.123233995736766e-17 0 -1 0.8102563866847022 -0.5860755820170724 4.961389452210604e-17 -0.5860755820170724 -0.8102563866847022 -3.588677927878149e-17 -1.210026345067984e+33 1.439375937748279e+33 -8.875825418086866e+32 98042843727.3678 51462236086.99052 -50204817501.65829 
-12 0.05155698431010575 0.01204569241719839 0.04321663039157932 -0.8523767713390271 -0.3422959498819833 0.3953319141886304 0.3743558812203692 0.1284275452859502 0.9183485393942465 -0.3651184929271272 0.9307737900833993 0.01867184555439828 -0.0001121160403309545 -0.003442066071130309 0.0001486793584100341 0.001360110999449947 -0.1433453458928777 -0.03556659637698854 
-13 2.078620893335288e+34 -3.343360758780396e+33 -3.733771264503461e+33 6.123233995736766e-17 0 1 -0.4935059172418994 -0.869742438683563 3.021852209552853e-17 0.869742438683563 -0.4935059172418994 -5.325636468082193e-17 2.482271609158022e+51 -1.106514996765646e+52 2.357433885238145e+52 -6.196964617030702e+16 -1.124204137849262e+18 -5.211455542051318e+17 
-14 0.04028795259638693 0.02151639379617646 0.04061543538836646 -0.7279735195962094 -0.6704757316243467 0.1432370345598662 0.567990550700566 -0.4727736823804364 0.6737000664712081 -0.3839808446805325 0.57179309068162 0.7249906015716242 0.0005522288819008228 7.198083003967284e-05 -0.001995232273071448 -0.07548223042174558 -0.08820407572220408 -0.03883336736897271 
-15 3.672775780860888e+21 -1.530415252460415e+22 2.570901802532228e+22 6.123233995736766e-17 0 1 0.1244903502823163 -0.9922208185109734 -7.622835449898575e-18 0.9922208185109734 0.1244903502823163 -6.075600247184153e-17 -1.683469418736864e+34 7.221744009293154e+33 6.686228210194867e+33 -371455031856.5137 -273762060984.9973 -639567212984.6284 
-16 0.03909572639280024 0.02189678957569788 0.03953197070024619 0.3647915749085239 -0.580501215883714 0.7279735195962065 0.7950909584619413 -0.2126431331760585 -0.5679905507005732 0.4845177753682833 0.7860033309538826 0.3839808446805271 0.0006825736939389619 3.725897022060023e-05 -0.002116854345175428 -0.01003323749104181 -0.1388010923392572 -0.005707004674076994 
-17 -9.58044157321739e+23 2.333762133743565e+24 1.149588215948051e+24 6.123233995736766e-17 0 1 -0.06713256979841288 0.9977440644134453 4.11068433610813e-18 -0.9977440644134453 -0.06713256979841288 6.109420374260982e-17 3.642631195273236e+36 1.571592456025881e+36 -1.533918044479e+35 -723160654816.3004 1600814021454.695 -771751275992.2809 
-18 0.006021862846385156 0.02507285670465683 0.1450414357296596 -0.6667980302136549 0.6294824764697375 -0.3989137735409948 -0.7265310009544569 -0.4299040512610118 0.5360365765145922 0.165930984313674 0.6472513565276596 0.7439977082745616 -0.01764063078443033 0.001098031301540985 -0.007777655078685334 0.4247586535357161 -0.309281973481961 -1.126873663303522 
-19 1.221759908611038e+31 -1.204474879168255e+31 9.069545709819155e+30 6.123233995736766e-17 0 -1 -0.2690292133762432 -0.9631320170932746 -1.647328825191732e-17 -0.9631320170932746 0.2690292133762432 -5.897482709448063e-17 2.522702761014422e+47 5.606305229215325e+47 4.036486616910206e+47 -3.059546811962792e+16 -2887737043743416 2.313219788656833e+16