Commit 7208ca49 authored by alexbuset's avatar alexbuset

mbs_close_loop

parent b80d34af
Pipeline #5332 failed with stage
in 0 seconds
......@@ -14,7 +14,8 @@
#include "MBSfun.h"
#include "nrfct.h"
#include "mbs_project_interface.h"
#include "mbs_message.h"
#include "mbs_part.h"
double norm_vector(double *v, int n);
double norminf_vector(double *v, int n);
......@@ -26,19 +27,22 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
int i,j;
int iter=0;
int nL,nC;
int err = 0;
double d;
double** mJv_0;
int* ind_c = NULL;
int* ind_u_des = NULL;
MbsPart *mbs_part;
iter = 0;
mbs_aux->norm_h=1.0;
while((mbs_aux->norm_h > mbs_aux->NRerr) && (iter++ <= mbs_aux->MAX_NR_ITER))
{
// Calcul des contraintes et de la Jacobienne
mbs_calc_hJ(s,mbs_aux);
// Norme des contraintes (en supposant que toutes les contraintes independantes sont au debut ???)
mbs_aux->norm_h = norminf_vector(mbs_aux->h,s->nhu);
......@@ -52,10 +56,120 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]];
}
}
ind_u_des = get_ivec_0(0);
ind_c = get_ivec_0(s->nqc);
sort_ivec_0(s->qc + 1, ind_c, s->nqc);
// switch to index strating at 0 (instead of 1)
for (i = 0; i < s->nqc; i++) {
ind_c[i] -= 1;
}
// -------------------- ALEX start ------------------
mJv_0 = get_dmat_0(nL, nC);
sub_matrix(mbs_aux->mJv, nL + 1, 0, 0, mJv_0);
//if (fabs(det_dmatrix_0(mJv_0, nL)) < mbs_aux->min_det || (iter >= 10 && iter % 10 == 0)) {
if (fabs(det_dmatrix_0(mJv_0, nL)) < 5e-3 && mbs_aux->online_part){
//if ((iter > 0 && iter % mbs_aux->iter_part == 0) && mbs_aux->online_part) {
mbs_part = mbs_new_part(s);
mbs_part->options->rowperm = 1;
mbs_part->options->verbose = 0;
if (mbs_aux->Jv_verbose) {
printf("mJv matrix before partitioning : \n");
print_dmat_0(mbs_aux->mJv, nC + 1, nL + 1);
}
s->DonePart = 0;
s->nqu = 0;
err = PART_coord_part(s, mbs_part, ind_u_des, 0, ind_c, s->nqc, 0);
if (err < 0) // Error management
{
mbs_msg(">>PART>>\n");
mbs_msg(">>PART>> ***** mbs_run_part : Coordinate partitioning interrupted : *****\n");
mbs_msg(">>PART>>\n");
//myproject_part = MBS_part;
s->DonePart = 0;
mbs_error_msg(" [%d] in mbs_run_part !! \n", -100 + err);
}
s->nqu = mbs_part->n_qu;
for (i = 0; i < mbs_part->n_qu; i++)
{
s->qu[i + 1] = mbs_part->ind_qu[i] + 1;
}
s->nqv = mbs_part->n_qv;
for (i = 0; i < mbs_part->n_qv; i++)
{
s->qv[i + 1] = mbs_part->ind_qv[i] + 1;
}
s->nhu = mbs_part->n_hu;
for (i = 0; i < mbs_part->n_hu; i++)
{
s->hu[i + 1] = mbs_part->ind_hu[i] + 1;
}
if (s->nqu + s->nqc > 0)
{
mbs_aux->iquc = get_ivec_1(s->nqu + s->nqc);
for (i = 1; i <= s->nqu; i++)
mbs_aux->iquc[i] = s->qu[i];
for (i = 1; i <= s->nqc; i++)
mbs_aux->iquc[i + s->nqu] = s->qc[i];
}
if (mbs_part->options->verbose)
{
mbs_msg(">>PART>> Partitionning result :\n");
mbs_msg("nqu : %d\n", mbs_part->n_qu);
mbs_msg("qu : "); print_ivec_0(s->qu + 1, mbs_part->n_qu);
mbs_msg("\n");
mbs_msg("nqv : %d\n", mbs_part->n_qv);
mbs_msg("qv : "); print_ivec_0(s->qv + 1, mbs_part->n_qv);
mbs_msg("\n");
mbs_msg("nhu : %d\n", mbs_part->n_hu);
mbs_msg("hu : "); print_ivec_0(s->hu + 1, mbs_part->n_hu);
mbs_msg("\n");
mbs_msg(">>PART>> ***** mbs_run_part end *****\n");
}
mbs_delete_part(mbs_part);
for (i = 1; i <= nL; i++)
{
for (j = 1; j <= nC; j++)
{
mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]];
}
}
if (mbs_aux->Jv_verbose) {
printf("mJv matrix after partitioning : \n");
print_dmat_0(mbs_aux->mJv, nC + 1, nL + 1);
}
mbs_aux->permu = 1;
return 12;
// -------------------- ALEX stop ------------------
}
// Decomposition LU de la matrice -Jv
ludcmp(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,&d);
err = ludcmp(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,&d);
if (err < 0) // Error management
{
mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during LU decomposition of Jv matrix \n");
return -50+err;
}
if(mbs_aux->norm_h > mbs_aux->NRerr)
{
// err
......@@ -64,6 +178,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
mbs_aux->mJv_h[i] = mbs_aux->h[s->hu[i]];
// mbs_aux->mJv_h[i][1] = mbs_aux->h[s->hu[i]];
}
lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->mJv_h);
// gaussj(mbs_aux->mJv, s->nqv, mbs_aux->mJv_h, 1);
......@@ -73,8 +188,18 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
s->q[s->qv[i]] += mbs_aux->mJv_h[i];
// s->q[s->qv[i]] += mbs_aux->mJv_h[i][1];
}
}
}
if (iter >= mbs_aux->MAX_NR_ITER) // Error management
{
mbs_msg("\t \t >CLOSE GEO> Impossible to close the geometry after %d iterations \n", iter);
return -50;
}
return iter;
}
......@@ -86,7 +211,7 @@ void mbs_close_kin(MbsData *s, MbsAux *mbs_aux)
double term;
nL = s->nhu;
nC = mbs_aux->iquc[0];
nC = s->nqu + s->nqc;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
......@@ -113,7 +238,7 @@ void mbs_close_kin(MbsData *s, MbsAux *mbs_aux)
// calcul des vitesses dependantes (qdv = Bvuc * qduc)
nL = s->nqv;
nk = mbs_aux->iquc[0];
nk = s->nqu + s->nqc;
for(i=1;i<=nL;i++)
{
term = 0.0;
......
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