29 lines
889 B
C++
29 lines
889 B
C++
#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);
|
|
}
|