Commit fd53795f authored by Olivier Lantsoght's avatar Olivier Lantsoght
Browse files

[New] Animate the closing procedure when a module failed to close the loops....

[New] Animate the closing procedure when a module failed to close the loops. Currently not computationally efficient.
parent ee2c282c
......@@ -15,6 +15,8 @@
#include "nrfct.h"
#include "mbs_project_interface.h"
#include "mbs_message.h"
#include "mbs_solvekin.h"
double norm_vector(double *v, int n);
double norminf_vector(double *v, int n);
......@@ -29,6 +31,13 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
int err = 0;
double d;
// Backup of qv
double *qv = get_dvec_1(s->nqv);
for(i=1;i<=s->nhu;i++)
{
qv[i] = s->q[s->qv[i]];
}
iter = 0;
mbs_aux->norm_h=1.0;
while((mbs_aux->norm_h > mbs_aux->NRerr) && (iter++ <= mbs_aux->MAX_NR_ITER))
......@@ -36,6 +45,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
err = mbs_step_close_geo(s, mbs_aux);
if (err <0){
mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during a NR iteration \n");
free_dvec_0(qv);
return -50+err;
}
}
......@@ -43,10 +53,12 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
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);
mbs_anim_close_geo(s, mbs_aux, qv);
free_dvec_0(qv);
return -50;
}
free_dvec_0(qv);
return iter;
}
......@@ -100,6 +112,25 @@ int mbs_step_close_geo(MbsData *s, MbsAux *mbs_aux){
return 0;
}
int mbs_anim_close_geo(MbsData *mbs_data, MbsAux *mbs_aux, double *qv){
mbs_msg("\t \t > Attempting to generate animation of the failed loop closing procedure.\n");
// Restoring qv
int i;
for(i=1;i<=mbs_data->nhu;i++)
{
mbs_data->q[mbs_data->qv[i]] = qv[i];
}
// Creating the animation with the solvekin module
MbsSolvekin *mbs_solvekin;
mbs_solvekin = mbs_new_solvekin(mbs_data);
mbs_solvekin->options->motion = closeloop;
mbs_solvekin->options->framerate = 1;
mbs_solvekin->options->resfilename = "Failed_loop_closing_procedure";
mbs_run_solvekin(mbs_solvekin, mbs_data);
mbs_delete_solvekin(mbs_solvekin, mbs_data);
mbs_msg("\t \t > Loop closure procedure animated in file \"Failed_loop_closing_procedure_q.anim\".\n");
}
void mbs_close_kin(MbsData *s, MbsAux *mbs_aux)
{
int i,j,k;
......
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