Commit 3dd3a8a8 authored by alexbuset's avatar alexbuset

rk4.c + useful_functions.c + useful_function.h oline part

parent adb7d524
Pipeline #5349 failed with stages
in 0 seconds
......@@ -25,6 +25,17 @@
int i;
double xh, hh, h6, *dym, *dyt, *yt;
//----------------- Modif Alex Start -----------------
double *q_save, *qd_save;
q_save = get_dvec_1(s->njoint);
qd_save = get_dvec_1(s->njoint);
copy_dvec_1(s->q, q_save);
copy_dvec_1(s->qd, qd_save);
//----------------- Modif Alex End -----------------
dym = &((MbsDirdynRK4 *)dd->integrator_struct)->dym[0];
dyt = &((MbsDirdynRK4 *)dd->integrator_struct)->dyt[0];
yt = &((MbsDirdynRK4 *)dd->integrator_struct)->yt[0];
......@@ -40,11 +51,39 @@
// Second step
(*derivs)(xh, yt, dyt, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
for (i = 0; i < n; i++)
yt[i] = y[i] + hh*dyt[i];
// Third step
(*derivs)(xh, yt, dym, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
for (i = 0; i < n; i++)
{
yt[i] = y[i] + h*dym[i];
......@@ -54,7 +93,22 @@
// Fourth step
(*derivs)(x + h, yt, dyt, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
// Accumulate increments with proper weights
for (i = 0; i < n; i++)
yout[i] = y[i] + h6*(dydx[i] + dyt[i] + 2.0*dym[i]);
free_dvec_1(q_save);
free_dvec_1(qd_save);
}
......@@ -1108,3 +1108,85 @@ void scale_matrix(double** matrix, int* n, int* initzeros)
}
}
}
//---------------------------------------------------------------------------
// Function definition to obtain a sub matrix without ligne 1 and colum x
//---------------------------------------------------------------------------
void sub_matrix(double** a, int n, int x, int y, double** a1) {
int i, j;
int k = 0;
int m = 0;
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
if (i != x && j != y) {
a1[m][k] = a[i][j];
k++;
}
}
k = 0;
if (i != x) {
m++;
}
}
}
//---------------------------------------------------------------------------
// Function definition to compute the determinant of a matrix
//---------------------------------------------------------------------------
/*
* a : matrix
* n : matrix's size
*/
double det_dmatrix_0(double** a, int n) {
int i, j, k, index;
double ratio, swap, det = 1.0, p = 1.0;
for (i = 0; i < n - 1; i++)
{
index = i;
while (a[index][i] == 0.0) {
index++;
if (index == n) {
index = 0;
// the determinat of the matrix is zero
while (a[index][i] == 0.0 && index < i) {
index++;
}
// if all the colone is null det(a) = 0
if (index == i) {
return 0.0;
}
// if not nothing to do with this i
else {
continue;
}
}
}
if (index != i)
{
//loop for swaping the diagonal element row and index row
for (j = 0; j < n; j++)
{
swap = a[index][j];
a[index][j] = a[i][j];
a[i][j] = swap;
}
//determinant sign changes when we shift rows
//go through determinant properties
p = -p;
}
for (j = i + 1; j < n; j++)
{
ratio = a[j][i] / a[i][i];
for (k = i; k < n; k++)
{
a[j][k] = a[j][k] - ratio * a[i][k];
}
}
det *= a[i][i];
}
return p * det*a[n - 1][n - 1];
}
......@@ -517,6 +517,24 @@ void arrange_matrix(double** matrix, int* n, int* initzeros);
*/
void scale_matrix(double** matrix, int* n, int* initzeros);
/*! \brief function to obtain a sub matrix without line x and colum y
*
* \param[in,out] the matrix
* \param[in] the size of the matrix nxn
* \param[in] the number of the line to skip
* \param[in] the number of the colum to skip
* \param[in, out] sub-matrix
*/
void sub_matrix(double** a, int n, int x, int y, double** a1);
/*! \brief function to compute the determinant of a matrix
*
* \param[in,out] the matrix
* \param[in] the size of the matrix nxn
* \return the determinant of this matrix
*/
double det_dmatrix_0(double** a, int n);
/*
* Print values of mbs_data (used for debug)
*/
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment