GearJoint and MBDynCase3orig.mbd

OndselSolver/AccNewtonRaphson.cpp

@@ -69,7 +69,7 @@ void AccNewtonRaphson::fillY()
 		item->fillAccICIterError(y);
 		//std::cout << item->name << *y << std::endl;
 		});
-	//std::cout << *y << std::endl;
+	//std::cout << "Final" << *y << std::endl;
 }
 
 void AccNewtonRaphson::incrementIterNo()

OndselSolver/ConstVelConstraintIJ.cpp

@@ -8,13 +8,22 @@
 
 #include "ConstVelConstraintIJ.h"
 #include "DirectionCosineIecJec.h"
+#include "ConstVelConstraintIqcJqc.h"
+#include "EndFrameqc.h"
 
 using namespace MbD;
 
 ConstVelConstraintIJ::ConstVelConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj) : ConstraintIJ(frmi, frmj)
 {
 }
 
+std::shared_ptr<ConstVelConstraintIJ> MbD::ConstVelConstraintIJ::With(EndFrmsptr frmi, EndFrmsptr frmj)
+{
+	assert(frmi->isEndFrameqc());
+	assert(frmj->isEndFrameqc());
+	return std::make_shared<ConstVelConstraintIqcJqc>(frmi, frmj);
+}
+
 void ConstVelConstraintIJ::calcPostDynCorrectorIteration()
 {
 	aG = aA01IeJe->aAijIeJe + aA10IeJe->aAijIeJe - aConstant;

OndselSolver/ConstVelConstraintIJ.h

@@ -19,6 +19,8 @@ namespace MbD {
     public:
         ConstVelConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj);
 
+        static std::shared_ptr<ConstVelConstraintIJ> With(EndFrmsptr frmi, EndFrmsptr frmj);
+
         void calcPostDynCorrectorIteration() override;
         virtual void initA01IeJe();
         virtual void initA10IeJe();

OndselSolver/ConstVelConstraintIqcJc.cpp

@@ -18,6 +18,8 @@ using namespace MbD;
 
 MbD::ConstVelConstraintIqcJc::ConstVelConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj) : ConstVelConstraintIJ(frmi, frmj)
 {
+	pGpEI = std::make_shared<FullRow<double>>(4);
+	ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
 }
 
 void MbD::ConstVelConstraintIqcJc::calcPostDynCorrectorIteration()

OndselSolver/ConstVelConstraintIqcJqc.cpp

@@ -15,6 +15,9 @@ using namespace MbD;
 
 MbD::ConstVelConstraintIqcJqc::ConstVelConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj) : ConstVelConstraintIqcJc(frmi, frmj)
 {
+	pGpEJ = std::make_shared<FullRow<double>>(4);
+	ppGpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
+	ppGpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
 }
 
 void MbD::ConstVelConstraintIqcJqc::calcPostDynCorrectorIteration()

OndselSolver/ConstantVelocityJoint.cpp

@@ -22,12 +22,25 @@ MbD::ConstantVelocityJoint::ConstantVelocityJoint(const char* str) : AtPointJoin
 {
 }
 
+void MbD::ConstantVelocityJoint::initializeLocally()
+{
+	if (!constraints->empty())
+	{
+		auto constraint = std::static_pointer_cast<ConstVelConstraintIJ>(constraints->back());
+		constraint->initA01IeJe();
+		constraint->initA10IeJe();
+	}
+	Joint::initializeLocally();
+}
+
 void MbD::ConstantVelocityJoint::initializeGlobally()
 {
 	if (constraints->empty())
 	{
 		createAtPointConstraints();
-		addConstraint(CREATE<ConstVelConstraintIJ>::With(frmI, frmJ));
+		auto constVelIJ = ConstVelConstraintIJ::With(frmI, frmJ);
+		constVelIJ->setConstant(0.0);
+		addConstraint(constVelIJ);
 		this->root()->hasChanged = true;
 	}
 	else {

OndselSolver/ConstantVelocityJoint.h

@@ -17,6 +17,7 @@ namespace MbD {
     public:
         ConstantVelocityJoint();
         ConstantVelocityJoint(const char* str);
+        void initializeLocally() override;
         void initializeGlobally() override;
         void connectsItoJ(EndFrmsptr frmI, EndFrmsptr frmJ) override;
 

OndselSolver/EndFramec.cpp

@@ -102,3 +102,8 @@ std::shared_ptr<EndFrameqc> MbD::EndFramec::newCopyEndFrameqc()
 	markerFrame->addEndFrame(frmIeqc);
 	return frmIeqc;
 }
+
+bool MbD::EndFramec::isEndFrameqc()
+{
+	return false;
+}

OndselSolver/EndFramec.h

@@ -41,6 +41,7 @@ namespace MbD {
 		virtual FColFMatDsptr pAOppE();
 		virtual FMatDsptr aBOp();
 		std::shared_ptr<EndFrameqc> newCopyEndFrameqc();
+		virtual bool isEndFrameqc();
 
 		MarkerFrame* markerFrame; //Use raw pointer when pointing backwards.
 		FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);

OndselSolver/EndFrameqc.cpp

@@ -155,3 +155,8 @@ FMatDsptr EndFrameqc::aBOp()
 {
 	return markerFrame->aBOp();
 }
+
+bool MbD::EndFrameqc::isEndFrameqc()
+{
+	return true;
+}

OndselSolver/EndFrameqc.h

@@ -40,6 +40,7 @@ namespace MbD {
         FColDsptr rpep() override;
         FColFMatDsptr pAOppE() override;
         FMatDsptr aBOp() override;
+        bool isEndFrameqc() override;
 
         FMatDsptr prOeOpE;
         FMatFColDsptr pprOeOpEpE;

OndselSolver/EndFrameqct.cpp

@@ -320,4 +320,8 @@ namespace MbD {
         pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
         ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
     }
+    bool EndFrameqct::isEndFrameqc()
+    {
+        return false;
+    }
 }

OndselSolver/EndFrameqct.h

@@ -49,8 +49,8 @@ namespace MbD {
 		virtual void evalppAmeptpt();
 		FColDsptr rmeO() override;
 		FColDsptr rpep() override;
-
 		void preAccIC() override;
+		bool isEndFrameqc() override;
 
 		std::shared_ptr<FullColumn<Symsptr>> rmemBlks, prmemptBlks, pprmemptptBlks;
 		std::shared_ptr<FullColumn<Symsptr>> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks;

OndselSolver/GearConstraintIJ.cpp

@@ -7,13 +7,22 @@
  ***************************************************************************/
 
 #include "GearConstraintIJ.h"
+#include "GearConstraintIqcJqc.h"
+#include "EndFrameqc.h"
 
 using namespace MbD;
 
 MbD::GearConstraintIJ::GearConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj) : ConstraintIJ(frmi, frmj)
 {
 }
 
+std::shared_ptr<GearConstraintIJ> MbD::GearConstraintIJ::With(EndFrmsptr frmi, EndFrmsptr frmj)
+{
+	assert(frmi->isEndFrameqc());
+	assert(frmj->isEndFrameqc());
+	return std::make_shared<GearConstraintIqcJqc>(frmi, frmj);
+}
+
 void MbD::GearConstraintIJ::calcPostDynCorrectorIteration()
 {
 	aG = orbitJeIe->value() + (this->ratio() * orbitIeJe->value()) - aConstant;

OndselSolver/GearConstraintIJ.h

@@ -18,6 +18,8 @@ namespace MbD {
 	public:
 		GearConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj);
 
+		static std::shared_ptr<GearConstraintIJ> With(EndFrmsptr frmi, EndFrmsptr frmj);
+
 		void calcPostDynCorrectorIteration() override;
 		void initialize() override;
 		void initializeGlobally() override;

OndselSolver/GearJoint.cpp

@@ -21,11 +21,21 @@ MbD::GearJoint::GearJoint(const char* str)
 {
 }
 
+void MbD::GearJoint::initializeLocally()
+{
+	if (!constraints->empty())
+	{
+		auto constraint = std::static_pointer_cast<GearConstraintIJ>(constraints->back());
+		constraint->initorbitsIJ();
+	}
+	Joint::initializeLocally();
+}
+
 void MbD::GearJoint::initializeGlobally()
 {
 	if (constraints->empty())
 	{
-		auto gearIJ = CREATE<GearConstraintIJ>::With(frmI, frmJ);
+		auto gearIJ = GearConstraintIJ::With(frmI, frmJ);
 		gearIJ->radiusI = radiusI;
 		gearIJ->radiusJ = radiusJ;
 		gearIJ->setConstant(std::numeric_limits<double>::min());

OndselSolver/GearJoint.h

@@ -17,6 +17,8 @@ namespace MbD {
 	public:
 		GearJoint();
 		GearJoint(const char* str);
+
+		void initializeLocally() override;
 		void initializeGlobally() override;
 
 		double radiusI = 0.0, radiusJ = 0.0, aConstant = 0.0;

OndselSolver/MBDynItem.cpp

@@ -201,7 +201,11 @@ FColDsptr MbD::MBDynItem::readPosition(std::vector<std::string>& args)
 	auto rOfO = std::make_shared<FullColumn<double>>(3);
 	if (args.empty()) return rOfO;
 	auto& str = args.at(0);
-	if (str.find("orientation") != std::string::npos) {
+	if (str.find("position") != std::string::npos) {
+		args.erase(args.begin());
+		rOfO = readBasicPosition(args);
+	}
+	else if (str.find("orientation") != std::string::npos) {
 		//Do nothing
 	}
 	else if (str.find("reference") != std::string::npos) {
@@ -248,6 +252,9 @@ FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
 		args.erase(args.begin());
 		aAOf = readOrientation(args);
 	}
+	else if (str.find("position") != std::string::npos) {
+		//Do nothing
+	}
 	else if (str.find("orientation") != std::string::npos) {
 		args.erase(args.begin());
 		aAOf = readOrientation(args);

OndselSolver/OndselSolver.vcxproj

@@ -763,20 +763,25 @@
   </ItemGroup>
   <ItemGroup>
     <None Include="..\testapp\00backhoe.asmt" />
+    <None Include="..\testapp\anglejoint.asmt" />
     <None Include="..\testapp\circular.asmt" />
     <None Include="..\testapp\cirpendu.asmt" />
     <None Include="..\testapp\cirpendu2.asmt" />
+    <None Include="..\testapp\constvel.asmt" />
     <None Include="..\testapp\CrankSlider2.mbd" />
     <None Include="..\testapp\crank_slider.mbd" />
     <None Include="..\testapp\crank_sliderX.mbd" />
     <None Include="..\testapp\engine1.asmt" />
     <None Include="..\testapp\fourbar.asmt" />
     <None Include="..\testapp\fourbot.asmt" />
+    <None Include="..\testapp\Gears.asmt" />
     <None Include="..\testapp\gyro.asmt" />
     <None Include="..\testapp\InitialConditions.mbd" />
     <None Include="..\testapp\MBDynCase.mbd" />
     <None Include="..\testapp\MBDynCase2.mbd" />
     <None Include="..\testapp\MBDynCase2orig.mbd" />
+    <None Include="..\testapp\MBDynCase3orig.mbd" />
+    <None Include="..\testapp\MBDynCase4orig.mbd" />
     <None Include="..\testapp\MBDynCaseDebug1.mbd" />
     <None Include="..\testapp\MBDynCaseDebug2.mbd" />
     <None Include="..\testapp\piston.asmt" />

OndselSolver/OndselSolver.vcxproj.filters

@@ -1911,6 +1911,11 @@
     <None Include="..\testapp\wobpump.asmt" />
     <None Include="..\testapp\planarbug.asmt" />
     <None Include="..\testapp\rackscrew.asmt" />
+    <None Include="..\testapp\constvel.asmt" />
+    <None Include="..\testapp\anglejoint.asmt" />
+    <None Include="..\testapp\Gears.asmt" />
+    <None Include="..\testapp\MBDynCase4orig.mbd" />
+    <None Include="..\testapp\MBDynCase3orig.mbd" />
   </ItemGroup>
   <ItemGroup>
     <Media Include="..\testapp\CrankSlider2.mov" />

OndselSolver/OrbitAngleZIecJec.cpp

@@ -30,8 +30,8 @@ void MbD::OrbitAngleZIecJec::calcPostDynCorrectorIteration()
 	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;
+	cosOverSSq = x / sumOfSquares;
+	sinOverSSq = y / sumOfSquares;
 	twoCosSinOverSSqSq = 2.0 * x * y / sumOfSquaresSquared;
 	dSqOverSSqSq = diffOfSquares / sumOfSquaresSquared;
 }

OndselSolver/PosICNewtonRaphson.cpp

@@ -5,7 +5,7 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #include <assert.h>
 #include <exception>
 
@@ -24,7 +24,12 @@ void PosICNewtonRaphson::run()
 {
 	while (true) {
 		try {
-			VectorNewtonRaphson::run();
+			//VectorNewtonRaphson::run();   //Inline to help debugging
+			preRun();
+			initializeLocally();
+			initializeGlobally();
+			iterate();
+			postRun();
 			break;
 		}
 		catch (SingularMatrixError ex) {
@@ -97,15 +102,15 @@ bool PosICNewtonRaphson::isConverged()
 void PosICNewtonRaphson::handleSingularMatrix()
 {
 	nSingularMatrixError++;
-	if (nSingularMatrixError == 1){
+	if (nSingularMatrixError == 1) {
 		this->lookForRedundantConstraints();
 		matrixSolver = this->matrixSolverClassNew();
 	}
 	else {
 		std::string str = typeid(*matrixSolver).name();
 		if (str.find("GESpMatParPvMarkoFast") != std::string::npos) {
-		matrixSolver = CREATE<GESpMatParPvPrecise>::With();
-		this->solveEquations();
+			matrixSolver = CREATE<GESpMatParPvPrecise>::With();
+			this->solveEquations();
 		}
 		else {
 			str = typeid(*matrixSolver).name();