systemSolver->runBasicKinematic();

MbDCode/StableBackwardDifference.cpp

@@ -0,0 +1,28 @@
+#include "StableBackwardDifference.h"
+
+void MbD::StableBackwardDifference::formTaylorMatrix()
+{
+	//This form is numerically more stable and is prefered over the full Taylor Matrix.
+	//For method order 3:
+	//| (t1 - t)	(t1 - t) ^ 2 / 2!	(t1 - t) ^ 3 / 3!|	|qd(t)  | = | q(t1) - q(t)	|
+	//|	(t2 - t)	(t2 - t) ^ 2 / 2!	(t2 - t) ^ 3 / 3!|	|qdd(t)	|	|q(t2) - q(t)	|
+	//|	(t3 - t)	(t3 - t) ^ 2 / 2!	(t3 - t) ^ 3 / 3!|	|qddd(t)|	|q(t3) - q(t)	|
+
+	this->instanciateTaylorMatrix();
+	for (int i = 0; i < order; i++)
+	{
+		this->formTaylorRowwithTimeNodederivative(i, i, 0);
+	}
+}
+
+void MbD::StableBackwardDifference::instanciateTaylorMatrix()
+{
+	if (taylorMatrix->empty() || (taylorMatrix->nrow() != (order))) {
+		taylorMatrix = std::make_shared<FullMatrix<double>>(order, order);
+	}
+}
+
+void MbD::StableBackwardDifference::formTaylorRowwithTimeNodederivative(int i, int ii, int k)
+{
+	assert(false);
+}