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

[refactoring] use of slct_Xvec, copy_Xvec, pick_and_place_dvec.

Remove unused argument from slct_dvec_1.
parent f09bb347
......@@ -103,20 +103,12 @@ int mbs_Rred_core(MbsAux *mbs_aux, MbsData *s) {
}
else
{
nL = mbs_aux->nquc;
for (i = 1; i <= nL; i++)
{
mbs_aux->Rruc[i] = mbs_aux->R[mbs_aux->iquc[i]];
}
slct_dvec_1(mbs_aux->R, mbs_aux->iquc, mbs_aux->nquc, mbs_aux->Rruc);
}
// Second reduction: [nqu+nqc x 1] --> [nqu x 1]
// (Simply a selection of the first qu element of Rruc) as they have been orded.
nL = s->nqu;
for (i = 1; i <= nL; i++)
{
mbs_aux->Rred[i] = mbs_aux->Rruc[i];
}
copy_dvec_0(mbs_aux->Rruc+1, mbs_aux->Rred+1, s->nqu);
for (i = 1; i <= s->nqu; i++)
{
......@@ -155,9 +147,7 @@ int mbs_Rred_core(MbsAux *mbs_aux, MbsData *s) {
lubksb(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, mbs_aux->Jvtlambda);
// Store Lagrange multipliers into MbsData
for(i = 1 ; i <= s->nhu ; i++){
s->lambda[s->hu[i]] = mbs_aux->Jvtlambda[i];
}
pick_and_place_dvec_1(mbs_aux->Jvtlambda, s->hu, s->nhu, s->lambda);
}
// lambdac (Qc) computation Qc = Rruc_c
......
......@@ -186,10 +186,7 @@ void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux)
}
}
// Retrieving independant constraint value
for (i = 1; i <= s->nhu; i++)
{
mbs_aux->mJv_h[i] = mbs_aux->h[s->hu[i]];
}
slct_dvec_1(mbs_aux->h, s->hu, s->nhu, mbs_aux->mJv_h);
}
void mbs_calc_jdqd(MbsData *s, MbsAux *mbs_aux)
......
......@@ -284,10 +284,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
}
else if (dd->options->flag_compute_Qc == COMPUTE_CUSTOM_QC)
{
for (i = 1; i <= mbs_data->njoint; i++)
{
dd->mbs_aux->compute_Qc_vec[i] = dd->options->compute_Qc[i];
}
copy_ivec_1(dd->options->compute_Qc, dd->mbs_aux->compute_Qc_vec, mbs_data->njoint);
}
else
{
......
......@@ -344,9 +344,7 @@ int dirdynared(MbsAux *mbs_aux, MbsData *s)
lubksb(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, mbs_aux->Jvtlambda);
// Store Lagrange multipliers into MbsData
for(i = 1 ; i <= s->nhu ; i++){
s->lambda[s->hu[i]] = mbs_aux->Jvtlambda[i];
}
pick_and_place_dvec_1(mbs_aux->Jvtlambda, s->hu, s->nhu, s->lambda);
}
// Qc = Mruc_cu * udd + Mruc_cc * qddc + Fruc_c
......
......@@ -916,10 +916,7 @@ void mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
}
lubksb_0(grads, eq->nxs, indx, xds); //
free_ivec_0(indx);
for (i = 0; i < eq->nxs; i++)
{
xd[eq->xs[i]] = xds[i]; // now xd contains f(x)/Japprox(x)
}
pick_and_place_dvec_0(xds, eq->xs, eq->nxs, xd); // now xd contains f(x)/Japprox(x)
// 3.3 softing (some crazy algorithm ... !) SHOULD NOT BE ACTIVATED AT THE MOMENT
if (eq->options->soft)
......@@ -1020,10 +1017,7 @@ void mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
if (eq->norm_pk > normR)
{
eq->norm_pk = normR;
for (i = 0; i < eq->nxs; i++)
{
eq->x[eq->xs[i]] = xs[i]; // put the value as no relaxation was performed... !
}
pick_and_place_dvec_0(xs, eq->xs, eq->nxs, eq->x);
}
}
else
......
......@@ -29,14 +29,12 @@ int invdynared(MbsAux *mbs_aux, MbsData *s)
if (s->nqu && !s->nqv) { /// case 1 : UNconstrained system
// 1: Tree-like system
for (i = 1; i <= s->nqu; i++) {
mbs_aux->Qact[s->qu[i]] = mbs_aux->Rred[i];
}
pick_and_place_dvec_1(mbs_aux->Rred, s->qu, s->nqu, mbs_aux->Qact);
}
else if (!s->nqu && !s->nqv) { /// case 2 : UNconstrained system, fully driven
// 2: Tree-like system, driven motion
copy_dvec_0(s->Qc+1, mbs_aux->Qact+1, s->nqc);
for (i = 1; i < s->nqc; i++) {
mbs_aux->Qact[i] = s->Qc[i];
s->Qc[i] = 0;
}
}
......@@ -45,9 +43,7 @@ int invdynared(MbsAux *mbs_aux, MbsData *s)
// 3: constrained system
if (same_ivec_1(s->qu, s->qa)) {
// Rred was computed like if qa == qu
for (i = 1; i <= s->nqu; i++) {
mbs_aux->Qact[s->qu[i]] = mbs_aux->Rred[i];
}
pick_and_place_dvec_1(mbs_aux->Rred, s->qu, s->nqu, mbs_aux->Qact);
}
else {
/* Solving system : Qact = A\b
......@@ -86,10 +82,7 @@ int invdynared(MbsAux *mbs_aux, MbsData *s)
}
}
for (i = 1; i <= s->nqa; i++)
{
mbs_aux->b[i] = mbs_aux->Rred[i];
}
copy_dvec_0(mbs_aux->Rred+1, mbs_aux->b+1, s->nqa);
// Switch case between over-iso-under actuated system
if (s->nqa == s->nqu) { // solution of the original equations: Qact(ind_a) = A\b;
......@@ -104,9 +97,7 @@ int invdynared(MbsAux *mbs_aux, MbsData *s)
}
lubksb(mbs_aux->A, s->nqa, mbs_aux->ind_A, mbs_aux->b);
for (i = 1; i <= s->nqa; i++) {
mbs_aux->Qact[s->qa[i]] = mbs_aux->b[i];
}
pick_and_place_dvec_1(mbs_aux->b, s->qa, s->nqa, mbs_aux->Qact);
}
else if (s->nqa > s->nqu) { //overactuated system
mbs_msg(">>INVDYN>> nqa are greather than nqu.\n");
......
......@@ -991,11 +991,11 @@ void pick_and_place_dvec_0(double* src, int* index, int n, double* dest)
}
}
void slct_dvec_1(double* vec1, int x1, int* vec2, int x2, double* vec3)
void slct_dvec_1(double* vec1, int* vec2, int x, double* vec3)
{
int i;
for (i = 1; i <= x2; i++)
for (i = 1; i <= x; i++)
{
vec3[i] = vec1[vec2[i]];
}
......
......@@ -489,12 +489,11 @@ void pick_and_place_dvec_0(double* src, int* index, int n, double* dest);
/*! \brief fill the third (x2 sized) vector by slicing the first (x1 sized) vector of doubles at the indexes specified in the second (x2 sized) vector, starting with index 1
*
* \param[in] vec1 the vector containing the original values of doubles to be sliced
* \param[in] x1 the size of the vector vec1
* \param[in] vec2 the vector containing the indexes of the slices
* \param[in] x2 the size of the vector vec2,
* \param[in] x the size of the vector vec2,
* \param[out] vec3 the sliced vector such as vec3[i] = vec1[vec2[i]], starting with index 1. The vector have to be already allocated to a sufficient (x2) size.
*/
void slct_dvec_1(double* vec1, int x1, int* vec2, int x2, double* vec3);
void slct_dvec_1(double* vec1, int* vec2, int x, double* vec3);
/*! \brief fill the third (x2 sized) vector by slicing the first (x1 sized) vector of doubles at the indexes specified in the second (x2 sized) vector, starting with index 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