Merge pull request #44 from Ondsel-Development/fixbranch

sliptonic · web-flow · commit 719f6eddb88b · 2023-12-13T15:52:05.000-06:00
Fixbranch
diff --git a/.gitignore b/.gitignore
@@ -34,6 +34,7 @@
 .vs
 x64/
 *.bak
+assembly.asmt
 
 build
 cmake-build-debug
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -46,6 +46,7 @@ set(ONDSELSOLVER_SRC
         OndselSolver/ArcSine.cpp
         OndselSolver/ArcTan.cpp
         OndselSolver/ArcTan2.cpp
+        OndselSolver/Arguments.cpp
         OndselSolver/ASMTAngleJoint.cpp
         OndselSolver/ASMTAnimationParameters.cpp
         OndselSolver/ASMTAssembly.cpp
@@ -83,6 +84,7 @@ set(ONDSELSOLVER_SRC
         OndselSolver/ASMTRefSurface.cpp
         OndselSolver/ASMTRevCylJoint.cpp
         OndselSolver/ASMTRevoluteJoint.cpp
+        OndselSolver/ASMTRevRevJoint.cpp
         OndselSolver/ASMTRotationalMotion.cpp
         OndselSolver/ASMTScrewJoint.cpp
         OndselSolver/ASMTSimulationParameters.cpp
@@ -231,6 +233,16 @@ set(ONDSELSOLVER_SRC
         OndselSolver/MatrixSolver.cpp
         OndselSolver/MaximumIterationError.cpp
         OndselSolver/MbDMath.cpp
+        OndselSolver/MBDynAxialRotationJoint.cpp
+        OndselSolver/MBDynClampJoint.cpp
+        OndselSolver/MBDynDriveHingeJoint.cpp
+        OndselSolver/MBDynInLineJoint.cpp
+        OndselSolver/MBDynInPlaneJoint.cpp
+        OndselSolver/MBDynPrismaticJoint.cpp
+        OndselSolver/MBDynRevoluteHingeJoint.cpp
+        OndselSolver/MBDynRevolutePinJoint.cpp
+        OndselSolver/MBDynSphericalHingeJoint.cpp
+        OndselSolver/MBDynTotalJoint.cpp
         OndselSolver/MBDynBlock.cpp
         OndselSolver/MBDynBody.cpp
         OndselSolver/MBDynControlData.cpp
@@ -284,6 +296,7 @@ set(ONDSELSOLVER_SRC
         OndselSolver/RedundantConstraint.cpp
         OndselSolver/RevCylJoint.cpp
         OndselSolver/RevoluteJoint.cpp
+        OndselSolver/RevRevJoint.cpp
         OndselSolver/ScalarNewtonRaphson.cpp
         OndselSolver/ScrewConstraintIJ.cpp
         OndselSolver/ScrewConstraintIqcJc.cpp
@@ -352,6 +365,7 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/ArcSine.h
         OndselSolver/ArcTan.h
         OndselSolver/ArcTan2.h
+        OndselSolver/Arguments.h
         OndselSolver/ASMTAngleJoint.h
         OndselSolver/ASMTAnimationParameters.h
         OndselSolver/ASMTAssembly.h
@@ -389,6 +403,7 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/ASMTRefSurface.h
         OndselSolver/ASMTRevCylJoint.h
         OndselSolver/ASMTRevoluteJoint.h
+        OndselSolver/ASMTRevRevJoint.h
         OndselSolver/ASMTRotationalMotion.h
         OndselSolver/ASMTScrewJoint.h
         OndselSolver/ASMTSimulationParameters.h
@@ -539,6 +554,16 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/MatrixSolver.h
         OndselSolver/MaximumIterationError.h
         OndselSolver/MbDMath.h
+        OndselSolver/MBDynAxialRotationJoint.h
+        OndselSolver/MBDynClampJoint.h
+        OndselSolver/MBDynDriveHingeJoint.h
+        OndselSolver/MBDynInLineJoint.h
+        OndselSolver/MBDynInPlaneJoint.h
+        OndselSolver/MBDynPrismaticJoint.h
+        OndselSolver/MBDynRevoluteHingeJoint.h
+        OndselSolver/MBDynRevolutePinJoint.h
+        OndselSolver/MBDynSphericalHingeJoint.h
+        OndselSolver/MBDynTotalJoint.h
         OndselSolver/MBDynBlock.h
         OndselSolver/MBDynBody.h
         OndselSolver/MBDynControlData.h
@@ -589,6 +614,7 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/resource.h
         OndselSolver/RevCylJoint.h
         OndselSolver/RevoluteJoint.h
+        OndselSolver/RevRevJoint.h
         OndselSolver/ScalarNewtonRaphson.h
         OndselSolver/ScrewConstraintIJ.h
         OndselSolver/ScrewConstraintIqcJc.h
diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp
@@ -56,6 +56,7 @@
 #include "ASMTRefSurface.h"
 #include "ExternalSystem.h"
 #include "SystemSolver.h"
+#include "ASMTRevRevJoint.h"
 
 using namespace MbD;
 
@@ -582,14 +583,17 @@ void MbD::ASMTAssembly::readJoints(std::vector<std::string>& lines)
 			joint = CREATE<ASMTUniversalJoint>::With();
 		}
 		//CompoundJoints
+		else if (jointsLines[0] == "\t\t\tSphSphJoint") {
+			joint = CREATE<ASMTSphSphJoint>::With();
+		}
 		else if (jointsLines[0] == "\t\t\tCylSphJoint") {
 			joint = CREATE<ASMTCylSphJoint>::With();
 		}
 		else if (jointsLines[0] == "\t\t\tRevCylJoint") {
 			joint = CREATE<ASMTRevCylJoint>::With();
 		}
-		else if (jointsLines[0] == "\t\t\tSphSphJoint") {
-			joint = CREATE<ASMTSphSphJoint>::With();
+		else if (jointsLines[0] == "\t\t\tRevRevJoint") {
+			joint = CREATE<ASMTRevRevJoint>::With();
 		}
 		//InLineJoints
 		else if (jointsLines[0] == "\t\t\tCylindricalJoint") {
diff --git a/OndselSolver/ASMTRevRevJoint.cpp b/OndselSolver/ASMTRevRevJoint.cpp
@@ -0,0 +1,18 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+#include <fstream>	
+
+#include "ASMTRevRevJoint.h"
+#include "RevRevJoint.h"
+
+using namespace MbD;
+
+std::shared_ptr<Joint> MbD::ASMTRevRevJoint::mbdClassNew()
+{
+    return CREATE<RevRevJoint>::With();
+}
diff --git a/OndselSolver/ASMTRevRevJoint.h b/OndselSolver/ASMTRevRevJoint.h
@@ -0,0 +1,22 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+
+#include "ASMTCompoundJoint.h"
+
+namespace MbD {
+    class ASMTRevRevJoint : public ASMTCompoundJoint
+    {
+        //
+    public:
+        std::shared_ptr<Joint> mbdClassNew() override;
+
+    };
+}
+
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 * OS_M_PI)) * (2.0 * OS_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;
@@ -91,6 +91,7 @@ void MbD::AngleZIecJec::prePosIC()
 {
 	aA00IeJe->prePosIC();
 	aA10IeJe->prePosIC();
+	assert(thez != std::numeric_limits<double>::min());
 	KinematicIeJe::prePosIC();
 }
 
diff --git a/OndselSolver/MBDynItem.h b/OndselSolver/MBDynItem.h
@@ -11,7 +11,6 @@
 
 namespace MbD {
 	class MBDynSystem;
-	class MBDynVariable;
 	class MBDynReference;
 	class MBDynNode;
 	class ASMTItem;
diff --git a/OndselSolver/MBDynSystem.h b/OndselSolver/MBDynSystem.h
@@ -18,7 +18,6 @@ namespace MbD {
     class MBDynControlData;
     class MBDynNode;
     class MBDynElement;
-    class MBDynVariable;
     class MBDynLabel;
     class MBDynReference;
     class MBDynDrive;
diff --git a/OndselSolver/Numeric.cpp b/OndselSolver/Numeric.cpp
@@ -19,35 +19,13 @@ double MbD::Numeric::arcTan0to2piYoverX(double y, double x)
 	//"(y/x) arcTan in the range 0 to 2*pi."
 	//"Double arcTan0to2piY: 1.0d overX: 1.0d."
 
-	if (x > 0.0) {
-		if (y >= 0) {
-			//"First quadrant."
-			return std::atan2(y, x);
-		}
-		else {
-			//"Forth quadrant."
-			return 2.0 * OS_M_PI + std::atan2(y, x);
-		}
+	if (y >= 0) {
+		//"First and second quadrants."
+		return std::atan2(y, x);
 	}
 	else {
-		if (x < 0.0) {
-			//"Second and Third quadrants."
-			return	OS_M_PI + std::atan2(y, x);
-		}
-		else {
-			//"x = 0"
-			if (y > 0.0) {
-				return OS_M_PI / 2.0;
-			}
-			else {
-				if (y < 0.0) {
-					return 1.5 * OS_M_PI;
-				}
-				else {
-					throw std::invalid_argument("atan2(0, 0) is not defined.");
-				}
-			}
-		}
+		//"Third and forth quadrants."
+		return 2.0 * OS_M_PI + std::atan2(y, x);
 	}
 }
 
diff --git a/OndselSolver/OndselSolver.vcxproj b/OndselSolver/OndselSolver.vcxproj
@@ -191,6 +191,7 @@
     <ClCompile Include="ASMTRefSurface.cpp" />
     <ClCompile Include="ASMTRevCylJoint.cpp" />
     <ClCompile Include="ASMTRevoluteJoint.cpp" />
+    <ClCompile Include="ASMTRevRevJoint.cpp" />
     <ClCompile Include="ASMTRotationalMotion.cpp" />
     <ClCompile Include="ASMTScrewJoint.cpp" />
     <ClCompile Include="ASMTSimulationParameters.cpp" />
@@ -405,6 +406,7 @@
     <ClCompile Include="RedundantConstraint.cpp" />
     <ClCompile Include="RevCylJoint.cpp" />
     <ClCompile Include="RevoluteJoint.cpp" />
+    <ClCompile Include="RevRevJoint.cpp" />
     <ClCompile Include="RowTypeMatrix.cpp" />
     <ClCompile Include="ScalarNewtonRaphson.cpp" />
     <ClCompile Include="ScrewConstraintIJ.cpp" />
@@ -513,6 +515,7 @@
     <ClInclude Include="ASMTRefSurface.h" />
     <ClInclude Include="ASMTRevCylJoint.h" />
     <ClInclude Include="ASMTRevoluteJoint.h" />
+    <ClInclude Include="ASMTRevRevJoint.h" />
     <ClInclude Include="ASMTRotationalMotion.h" />
     <ClInclude Include="ASMTScrewJoint.h" />
     <ClInclude Include="ASMTSimulationParameters.h" />
@@ -729,6 +732,7 @@
     <ClInclude Include="resource.h" />
     <ClInclude Include="RevCylJoint.h" />
     <ClInclude Include="RevoluteJoint.h" />
+    <ClInclude Include="RevRevJoint.h" />
     <ClInclude Include="RowTypeMatrix.h" />
     <ClInclude Include="ScalarNewtonRaphson.h" />
     <ClInclude Include="ScrewConstraintIJ.h" />
diff --git a/OndselSolver/OrbitAngleZIecJec.cpp b/OndselSolver/OrbitAngleZIecJec.cpp
@@ -58,13 +58,15 @@ void MbD::OrbitAngleZIecJec::postInput()
 {
 	xIeJeIe->postInput();
 	yIeJeIe->postInput();
-	auto x = xIeJeIe->value();
-	auto y = yIeJeIe->value();
-	if (x > 0.0) {
-		thez = std::atan2(y, x);
-	}
-	else {
-		thez = Numeric::arcTan0to2piYoverX(y, x);
+	if (thez == std::numeric_limits<double>::min()) {
+		auto x = xIeJeIe->value();
+		auto y = yIeJeIe->value();
+		if (x > 0.0) {
+			thez = std::atan2(y, x);
+		}
+		else {
+			thez = Numeric::arcTan0to2piYoverX(y, x);
+		}
 	}
 	KinematicIeJe::postInput();
 }
@@ -78,7 +80,7 @@ void MbD::OrbitAngleZIecJec::postPosICIteration()
 
 void MbD::OrbitAngleZIecJec::preAccIC()
 {
-	if (thez == 0.0) this->prePosIC();
+	if (thez == std::numeric_limits<double>::min()) this->prePosIC();
 	xIeJeIe->preAccIC();
 	yIeJeIe->preAccIC();
 	KinematicIeJe::preAccIC();
@@ -88,14 +90,7 @@ void MbD::OrbitAngleZIecJec::prePosIC()
 {
 	xIeJeIe->prePosIC();
 	yIeJeIe->prePosIC();
-	auto x = xIeJeIe->value();
-	auto y = yIeJeIe->value();
-	if (x > 0.0) {
-		thez = std::atan2(y, x);
-	}
-	else {
-		thez = Numeric::arcTan0to2piYoverX(y, x);
-	}
+	assert(thez != std::numeric_limits<double>::min());
 	KinematicIeJe::prePosIC();
 }
 
diff --git a/OndselSolver/OrbitAngleZIecJec.h b/OndselSolver/OrbitAngleZIecJec.h
@@ -32,7 +32,8 @@ namespace MbD {
         void simUpdateAll() override;
         double value() override;
 
-        double thez, cosOverSSq, sinOverSSq, twoCosSinOverSSqSq, dSqOverSSqSq;
+        double thez = std::numeric_limits<double>::min();
+        double cosOverSSq, sinOverSSq, twoCosSinOverSSqSq, dSqOverSSqSq;
         std::shared_ptr<DispCompIecJecIe> xIeJeIe, yIeJeIe;
     };
 }
diff --git a/OndselSolver/RevRevJoint.cpp b/OndselSolver/RevRevJoint.cpp
@@ -0,0 +1,39 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "RevRevJoint.h"
+#include "CREATE.h"
+#include "DistancexyConstraintIJ.h"
+#include "System.h"
+
+using namespace MbD;
+
+MbD::RevRevJoint::RevRevJoint()
+{
+}
+
+MbD::RevRevJoint::RevRevJoint(const char* str) : CompoundJoint(str)
+{
+}
+
+void MbD::RevRevJoint::initializeGlobally()
+{
+	if (constraints->empty())
+	{
+		auto distxyIJ = DistancexyConstraintIJ::With(frmI, frmJ);
+		distxyIJ->setConstant(distanceIJ);
+		addConstraint(distxyIJ);
+		addConstraint(CREATE<TranslationConstraintIJ>::ConstraintWith(frmI, frmJ, 2));
+		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 0));
+		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 1));
+		this->root()->hasChanged = true;
+	}
+	else {
+		CompoundJoint::initializeGlobally();
+	}
+}
diff --git a/OndselSolver/RevRevJoint.h b/OndselSolver/RevRevJoint.h
@@ -0,0 +1,25 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+
+#include "CompoundJoint.h"
+
+namespace MbD {
+	class RevRevJoint : public CompoundJoint
+	{
+		//
+	public:
+		RevRevJoint();
+		RevRevJoint(const char* str);
+		void initializeGlobally() override;
+
+
+	};
+}
+
diff --git a/OndselSolver/ScrewConstraintIJ.cpp b/OndselSolver/ScrewConstraintIJ.cpp
@@ -31,7 +31,7 @@ std::shared_ptr<ScrewConstraintIJ> MbD::ScrewConstraintIJ::With(EndFrmsptr frmi,
 void MbD::ScrewConstraintIJ::calcPostDynCorrectorIteration()
 {
 	auto z = zIeJeIe->value();
-	auto thez = thezIeJe->value();
+	auto thez = thezIeJe->thez;
 	aG = (2.0 * OS_M_PI * z) - (pitch * thez) - aConstant;
 }
 
diff --git a/OndselSolver/ScrewConstraintIqcJc.cpp b/OndselSolver/ScrewConstraintIqcJc.cpp
diff --git a/OndselSolver/assembly.asmt b/OndselSolver/assembly.asmt
diff --git a/testapp/OndselSolver.cpp b/testapp/OndselSolver.cpp
diff --git a/testapp/RevRevJt.asmt b/testapp/RevRevJt.asmt
