Example #1
0
void PART_calc_hJ(MBSdataStruct* MBSdata, double* h, double** J)
{
	int i,j;

	double* h_loopc = get_double_vec(MBSdata->Nloopc +1); 
	double** J_loopc = get_double_tab(MBSdata->Nloopc +1, MBSdata->njoint+1); 
	double* h_userc= get_double_vec(MBSdata->Nuserc +1); 
	double** J_userc = get_double_tab(MBSdata->Nuserc +1, MBSdata->njoint+1);

	if(MBSdata->Nloopc)
	{
		cons_hJ(h_loopc, J_loopc, MBSdata, MBSdata->tsim);
		for (i=0; i<MBSdata->Nloopc; i++) 
		{
			h[i] = h_loopc[i+1];
			for (j=0; j<MBSdata->njoint; j++)
			{
				J[i][j] =J_loopc[i+1][j+1];
			}
		}
	}
	if(MBSdata->Nuserc) 
	{
		user_cons_hJ(h_userc, J_userc, MBSdata, MBSdata->tsim);
		for (i=0; i<MBSdata->Nuserc; i++) 
		{
			h[MBSdata->Nloopc+i] = h_userc[i+1];
			for (j=0; j<MBSdata->njoint; j++)
			{
				J[MBSdata->Nloopc+i][j] =J_userc[i+1][j+1];
			}
		}
	}

	free_double_vec(h_loopc);
	free_double_tab(J_loopc, MBSdata->Nloopc +1);
	free_double_vec(h_userc);
	free_double_tab(J_userc, MBSdata->Nuserc +1);
}
Example #2
0
/*
 * Initialize a 'Write_files' structure
 */
Write_files* init_write_files(int nstep, int njoint)
{
    Write_files* write_files;

    write_files = (Write_files*) malloc(sizeof(Write_files));

    write_files->t = get_double_vec(nstep);

    write_files->qq = get_double_tab(njoint, nstep);

    write_files->out_vec = get_double_tab(NB_OUTPUT_VEC, nstep);

    write_files->kount = 0;

    return write_files;
} 
Example #3
0
void arrange_matrix(double** matrix,int* n,int* initzeros)
{
	int l,reqrow,i,k,lastrow,tempvar,large;
	double* rowtemp;
	rowtemp = get_double_vec(n[1]);
	lastrow=n[0]-1;

	for(l=0;l<n[0];++l)
	{
		large=initzeros[0];
		for(i=0;i<n[0];++i)
		{
			if(large<=initzeros[i])
			{
				large=initzeros[i];
				reqrow=i;
			}
		}

		initzeros[reqrow]=-1;
		tempvar=initzeros[reqrow];
		initzeros[reqrow]=initzeros[lastrow];
		initzeros[lastrow]=tempvar;

		for(k=0;k<n[1];++k)
		{
			rowtemp[k]=matrix[lastrow][k];
		}
		for(k=0;k<n[1];++k)
		{
			matrix[lastrow][k]=matrix[reqrow][k];
		}
		for(k=0;k<n[1];++k)
		{
			matrix[reqrow][k]=rowtemp[k];
		}
		lastrow=lastrow-1;
	}
}
Example #4
0
PART_gen_strct* init_PART_gen_strct(MDS_gen_strct*  mds_gen_strct)
{
	PART_gen_strct *part_gen_strct; 

	part_gen_strct = (PART_gen_strct*) malloc(sizeof(PART_gen_strct));

	part_gen_strct->options = init_PART_option_strct();

	part_gen_strct->n_qu = 0;
	part_gen_strct->ind_qu = NULL;

	part_gen_strct->n_qv = 0;
	part_gen_strct->ind_qv = NULL;

	part_gen_strct->n_hu = 0;
	part_gen_strct->ind_hu = NULL;

	part_gen_strct->n_hv = 0;
	part_gen_strct->ind_hv = NULL;

	part_gen_strct->q_closed = get_double_vec(mds_gen_strct->bodytree->n_joint);

	return part_gen_strct;
}
Example #5
0
void PART_coord_part(MDS_gen_strct*  mds_gen_strct, MBSdataStruct* MBSdata, PART_gen_strct*  part_gen_strct, int* ind_u_des, int n_u_des, int* ind_c, int n_c, int first_partitioning , int* err)
{
	int i, j, nl_JAC, nc_JAC, Real_Rank, prsnt,n_vu, ivu, irk, ierr; 
	
	double* q_rand, * q_safe; 
	double* h_; 
	double** Jac, **Jv;

	int *ind_u_des_com,  *ind_vu, *row_perm, *col_perm;
	int *ind_aux, *ind_irk, *ind_u_nsort ,*ind_hv_nsort; 

	q_rand = get_double_vec(MBSdata->njoint);
	q_safe = get_double_vec(MBSdata->njoint);

	nl_JAC = MBSdata->Nloopc + MBSdata->Nuserc;
	nc_JAC = MBSdata->njoint;

	h_ = get_double_vec(nl_JAC); 
	Jac = get_double_tab(nl_JAC, nc_JAC);

	ind_u_des_com = get_int_vec(n_u_des + n_c);
	n_vu = MBSdata->njoint - (n_u_des + n_c);
	ind_vu = get_int_vec(n_vu);

	*err = 0;

	// Calcul du rang réel de la matrice Jacobienne

	for(i=0; i<MBSdata->njoint; i++)
	{
		q_rand[i] = frand();
	}
	copy_double_vec(&(MBSdata->q[1]), q_safe, MBSdata->njoint);
	copy_double_vec(q_rand, &(MBSdata->q[1]), MBSdata->njoint);

	PART_calc_hJ(MBSdata, h_, Jac);

	Real_Rank = rank_double_tab(Jac, nl_JAC, nc_JAC);                   // calcul du rang reel de Jac pour q random

	copy_double_vec(q_safe, &(MBSdata->q[1]), MBSdata->njoint);

	//  Proposition de partitionnement pour un u_des fourni (vide), (partiel, plein, valide ou non)

	conc_int_vec (ind_u_des, n_u_des, ind_c, n_c, ind_u_des_com);    // mise en commun des u_des et des c
		
	ivu=0; // variables (v et u) a partitionner //old = setdiff in matlab
	for(i=0; i<MBSdata->njoint; i++)
	{
		prsnt = 0;
		for(j=0; j<(n_u_des + n_c) ;j++)
		{
			if(i == ind_u_des_com[j])
			{
				prsnt = 1;
			}
		}
		if(prsnt == 0)
		{
			ind_vu[ivu] = i;
			ivu++;
		}
	}

	if( first_partitioning == 1)// on ne perturbe les 0 que la première fois (car seconde = système fermé)
	{
		for(i=0; i<n_vu; i++)// perturbation random (10-5) des valeurs nulles des (v,u_non_des): éviter les singularités
			{
			if (fabs(MBSdata->q[ind_vu[i]]) < 1e-6)
			{
				MBSdata->q[ind_vu[i]] = 1e-5*frand();
			}
		}
	}

	PART_calc_hJ(MBSdata, h_, Jac);

	row_perm = get_int_vec(nl_JAC);
	col_perm = get_int_vec(n_vu);
	Jv = get_double_tab(nl_JAC, n_vu);

	// Pivoted LU Factorisation

	slctc_double_tab(Jac, nl_JAC, nc_JAC, Jv, n_vu, ind_vu);

	PART_lutot(Jv, nl_JAC, n_vu, part_gen_strct->options->rowperm, &irk, &ierr, row_perm, col_perm);

	// Check the user choice for independent coordinates

	if (irk < Real_Rank)   // choix mauvais - erreur
	{
		printf("<<<<<ok : %d>>>>>\n",irk );
		part_gen_strct->ind_qu = NULL; part_gen_strct->ind_qv = NULL; part_gen_strct->ind_hu = NULL; part_gen_strct->ind_hv = NULL;
		part_gen_strct->n_qu = 0;
		part_gen_strct->n_qv = 0;
		part_gen_strct->n_hu = 0;
		part_gen_strct->n_hv = 0;
		printf(">>PART>>\n");
		printf(">>PART>> mbs_coord_part: wrong choice for the independent variable set - Jv matrix singular\n");
		printf(">>PART>>\n");
		*err = -1;
	}
	else                 // choix valide
	{
		ind_aux = get_int_vec(n_vu);
		ind_irk = get_int_vec(irk);
		f0123_int_vec(ind_irk, irk);

		slct_int_vec(ind_vu, n_vu, col_perm, n_vu, ind_aux);

		part_gen_strct->n_qv = irk;
		part_gen_strct->ind_qv = get_int_vec(part_gen_strct->n_qv);

		slct_int_vec(ind_aux, 0, ind_irk, irk, part_gen_strct->ind_qv); // Liste definitive des v 
		

		if (n_vu > irk)    // additional independent variabes : les dernières                                      //?? n_vu = lenth (ind_aux)
		{
			ind_u_nsort = get_int_vec(n_u_des+(n_vu-irk));
			copy_int_vec(ind_u_des, ind_u_nsort,n_u_des);
			for(i=0; i<(n_vu-irk); i++)
			{
				ind_u_nsort[n_u_des + i] = ind_aux[irk+i];
			}
			part_gen_strct->n_qu = n_u_des +(n_vu-irk);
		}
		else
		{
			part_gen_strct->n_qu = n_u_des;
			ind_u_nsort = get_int_vec(part_gen_strct->n_qu);
			copy_int_vec(ind_u_des, ind_u_nsort,part_gen_strct->n_qu);         // no additional independent variable
		}
		part_gen_strct->ind_qu = get_int_vec(part_gen_strct->n_qu);
		sort_int_vec(ind_u_nsort, part_gen_strct->ind_qu, part_gen_strct->n_qu);
		free_int_vec(ind_u_nsort);

		for(i=0; i<part_gen_strct->n_qu; i++)
		{
			MBSdata->q[part_gen_strct->ind_qu[i]+1] = q_safe[part_gen_strct->ind_qu[i]]; // +1 for MBSdata convention 
		}

		if (irk == nl_JAC)                   // Case : no redundant constraints
		{
			part_gen_strct->n_hu = nl_JAC;
			part_gen_strct->ind_hu = get_int_vec(part_gen_strct->n_hu);
			copy_int_vec(row_perm, part_gen_strct->ind_hu, part_gen_strct->n_hu);
			
			part_gen_strct->ind_hv = NULL;
			part_gen_strct->n_hv = 0;
		}
		else      // Case : redundant constraints
		{
			part_gen_strct->n_hu = irk;
			part_gen_strct->ind_hu = get_int_vec(part_gen_strct->n_hu);
			copy_int_vec(row_perm, part_gen_strct->ind_hu, part_gen_strct->n_hu);

			part_gen_strct->n_hv = nl_JAC-irk;
			ind_hv_nsort = get_int_vec(part_gen_strct->n_hv);

			copy_int_vec(&(row_perm[irk]), ind_hv_nsort, part_gen_strct->n_hv);
			part_gen_strct->ind_hv = get_int_vec(part_gen_strct->n_hv);
			sort_int_vec(ind_hv_nsort, part_gen_strct->ind_hv, part_gen_strct->n_hv);

			free_int_vec(ind_hv_nsort);
		}
		free_int_vec(ind_aux);
		free_int_vec(ind_irk );
	}

	free_int_vec(row_perm);
	free_int_vec(col_perm);
	free_double_tab(Jv, nl_JAC);

	free_double_vec(q_rand);
	free_double_vec(q_safe);
	free_double_vec(h_); 
	free_double_tab(Jac, nl_JAC);
	free_int_vec(ind_u_des_com);
	free_int_vec(ind_vu);
}