DOUBLESQUARE_GRID *read_doublesquare_grid (DOUBLERASTER_MAP *draster, char *keyname,long (*index_pixel_from_a_bin_coarse)(long r, long c,LONGVECTOR *s_index),long (*index_pixel_from_a_bin_fine)(long r, long c,LONGVECTOR *s_index),short print){
/*
*
* \author Emanuele Cordano
* \date May 2009
*
*\param - DOUBLERASTER_MAP *draster
*\param - (char *) root neme of textfiles containing the struct information.
*\param long (*index_pixel_from_a_bin_coarse)(long r, long c,LONGVECTOR *s_index) - equation of the filling curve for the pixel of a coarse grid
*\param long (*index_pixel_from_a_bin_fine)(long r, long c,LONGVECTOR *s_index),long d_coarse,long d_fine,short print) - equation of the filling curve for the pixel of a fine grid
*\param (short) print
*
*/
DOUBLESQUARE_GRID *dsq;
char *keyname_coarse=join_strings(keyname,"__coarse");
char *keyname_fine=join_strings(keyname,"__fine");
char *filename_c_polygon=join_strings(keyname,"_c_polygon.txt");
char *filename_c_line=join_strings(keyname,"_c_line.txt");
dsq=(DOUBLESQUARE_GRID *)malloc(sizeof(DOUBLESQUARE_GRID));
if (!dsq) t_error("Double Square Grid dsq in read_doublesquare_grid was not allocated");
dsq->big=read_square_grid(keyname_coarse,draster->coarse->layer[draster->coarse->reference_index_map],index_pixel_from_a_bin_coarse,draster->coarse->UV->V,draster->coarse->check_novalues,print);
dsq->fine=read_square_grid(keyname_fine,draster->fine->layer[draster->fine->reference_index_map],index_pixel_from_a_bin_fine,draster->fine->UV->V,draster->fine->check_novalues,print);
dsq->file_resume_c_line=copy_string(filename_c_line);
dsq->file_resume_c_polygon=copy_string(filename_c_polygon);
dsq->small_content_line=read_line_indices(dsq->file_resume_c_line,print);
dsq->small_content_polygon=read_fine_indices(dsq->file_resume_c_polygon,print);
if (dsq->small_content_line->index->nh!=dsq->big->grid->lines->nh) {
printf("Error in read_doublesquare_grid inconstancy with number of (big) lines %ld and %ld \n",dsq->small_content_line->index->nh,dsq->big->grid->lines->nh);
stop_execution();
}
if (dsq->small_content_polygon->nh!=dsq->big->grid->polygons->nh) {
printf("Error in read_doublesquare_grid inconstancy with number of (big) lines %ld and %ld \n",dsq->small_content_polygon->nh,dsq->big->grid->polygons->nh);
stop_execution();
}
if (print==1) printf("Function read_doublesquare_grid was successfully executed! \n ");
return dsq;
}
void ExecutionManager::mode_callback(const tangible_msgs::Mode::ConstPtr& mode_msg)
{
mode = mode_msg->mode;
switch(mode)
{
case tangible_msgs::Mode::IDLE:
ROS_INFO("Idle Mode");
// stop all movements
stop_execution();
break;
case tangible_msgs::Mode::EDIT:
ROS_INFO("Edit Mode");
// stop all movements
stop_execution();
// clear the program. A new program should be obtained after the edit
// NOTE: this requires the compilation node to always hold on to the last valid program
clear_program();
break;
case tangible_msgs::Mode::EXECUTE:
ROS_INFO("Execution Mode");
// a program is a sequence of operations. If the program is not defined, obtain
// the program. Else, resume its execution from the first incompelete instruction.
// NOTE: instructions are the atomic units of normal program execution.
if(program.empty())
get_program();
start_execution();
break;
default:
ROS_ERROR("Invalid Mode");
break;
}
}
long jacobi_preconditioned_conjugate_gradient_search(long icnt, double epsilon, DOUBLEVECTOR *x, DOUBLEVECTOR *b, t_Matrix_element funz) {
/*!
*\param icnt - (long) number of reiterations
*\param epsilon - (double) required tollerance (2-order norm of the residuals)
*\param x - (DOUBLEVECTOR *) vector of the unknowns x in Ax=b
*\param b - (DOUBLEVECTOR *) vector of b in Ax=b
*\param funz - (t_Matrix_element) - (int) pointer to the application A (x and y doublevector y=A(param)x ) it return 0 in case of success, -1 otherwise.
*
*
*\brief algorithm proposed by Jonathan Richard Shewckuck in http://www.cs.cmu.edu/~jrs/jrspapers.html#cg and http://www.cs.cmu.edu/~quake-papers/painless-conjugate-gradient.pdf
*
* \author Emanuele Cordano
* \date June 2009
*
*\return the number of reitarations
*/
double delta,delta_new,alpha,beta,delta0;
DOUBLEVECTOR *r, *d,*q,*y,*sr,*diag;
int sl;
long icnt_max;
long j;
double p;
r=new_doublevector(x->nh);
d=new_doublevector(x->nh);
q=new_doublevector(x->nh);
y=new_doublevector(x->nh);
sr=new_doublevector(x->nh);
diag=new_doublevector(x->nh);
icnt=0;
icnt_max=x->nh;
for (j=x->nl; j<=x->nh; j++) {
y->element[j]=(*funz)(j,x);
}
get_diagonal(diag,funz);
// print_doublevector_elements(diag,PRINT);
// stop_execution();
delta_new=0.0;
for (j=y->nl; j<=y->nh; j++) {
r->element[j]=b->element[j]-y->element[j];
if (diag->element[j]<0.0) {
diag->element[j]=1.0;
printf("\n Error in jacobi_preconditioned_conjugate_gradient_search function: diagonal of the matrix (%lf) is negative at %ld \n",diag->element[j],j);
stop_execution();
}
// diag->element[j]=fmax(diag->element[j],MAX_VALUE_DIAG*fabs(r->element[j]));
//d->element[j]=r->element[j]/diag->element[j];
if (diag->element[j]==0.0) { //ec 20100315
d->element[j]=0.0;
} else {
d->element[j]=r->element[j]/(diag->element[j]);
}
delta_new+=r->element[j]*d->element[j];
}
// printf("delta0 =%le", delta0);
// double epsilon0=epsilon;
// double pe=5.0;
while ((icnt<=icnt_max) && (max_doublevector(r)>epsilon)) {
delta=delta_new;
// s=(* funz)(q,d);
p=0.0;
for(j=q->nl; j<=q->nh; j++) {
q->element[j]=(*funz)(j,d);
p+=q->element[j]*d->element[j];
}
alpha=delta_new/p;
for(j=x->nl; j<=x->nh; j++) {
x->element[j]=x->element[j]+alpha*d->element[j];
}
delta_new=0.0;
sl=0;
for (j=y->nl; j<=y->nh; j++) {
if (icnt%MAX_REITERTION==0) {
y->element[j]=(*funz)(j,x);
r->element[j]=b->element[j]-y->element[j];
} else {
r->element[j]=r->element[j]-alpha*q->element[j];
}
if (diag->element[j]==0.0) { // ec_20100315
sr->element[j]=0.0;
d->element[j]=0.0;
} else {
sr->element[j]=r->element[j]/diag->element[j];
}
delta_new+=sr->element[j]*r->element[j];
/* if (((j==y->nl) && sl==0 ) || (sl==1)) {
printf("delta_new =%le (j=%ld) ",delta_new,j);
// if (delta_new==0.0) sl=1;
}*/
}
beta=delta_new/delta;
// double aa=1.0e-21;
// ec Initial residual: printf("delta_new =%le p=%le alpha=%le beta=%le delta_max=%le\n",delta_new,p,alpha,beta,max_doublevector(r));
for (j=d->nl; j<=d->nh; j++) {
d->element[j]=sr->element[j]+beta*d->element[j];
}
icnt++;
}
free_doublevector(diag);
free_doublevector(sr);
free_doublevector(r);
free_doublevector(d);
free_doublevector(q);
free_doublevector(y);
return icnt;
}
long CG(double tol_rel, double tol_min, double tol_max, DOUBLEVECTOR *x, DOUBLEVECTOR *x0, double dt,
DOUBLEVECTOR *b, t_Matrix_element_with_voidp function, void *data){
/*!
*\param icnt - (long) number of reiterations
*\param epsilon - (double) required tollerance (2-order norm of the residuals)
*\param x - (DOUBLEVECTOR *) vector of the unknowns x in Ax=b
*\param b - (DOUBLEVECTOR *) vector of b in Ax=b
*\param funz - (t_Matrix_element_with_voidp) - (int) pointer to the application A (x and y doublevector y=A(param)x ) it return 0 in case of success, -1 otherwise.
*\param data - (void *) data and parameters related to the argurment t_Matrix_element_with_voidp funz
*
*
*\brief algorithm proposed by Jonathan Richard Shewckuck in http://www.cs.cmu.edu/~jrs/jrspapers.html#cg and http://www.cs.cmu.edu/~quake-papers/painless-conjugate-gradient.pdf
*
* \author Emanuele Cordano
* \date June 2009
*
*\return the number of reitarations
*/
double delta,alpha,beta,delta_new;
DOUBLEVECTOR *r, *d,*q,*y,*sr,*diag,*udiag;
int sl;
long icnt_max;
long icnt;
long j;
double p;
double norm_r0;
r=new_doublevector(x->nh);
d=new_doublevector(x->nh);
q=new_doublevector(x->nh);
y=new_doublevector(x->nh);
sr=new_doublevector(x->nh);
diag=new_doublevector(x->nh);
udiag=new_doublevector(x->nh-1);
icnt=0;
icnt_max=x->nh;
//icnt_max=(long)(sqrt((double)x->nh));
for (j=x->nl;j<=x->nh;j++){
y->co[j]=(*function)(j,x,x0,dt,data);
}
get_diagonal(diag,x0,dt,function,data);
get_upper_diagonal(udiag,x0,dt,function,data);
delta_new=0.0;
for (j=y->nl;j<=y->nh;j++) {
r->co[j]=b->co[j]-y->co[j];
if (diag->co[j]<0.0) {
diag->co[j]=1.0;
printf("\n Error in jacobi_preconditioned_conjugate_gradient_search function: diagonal of the matrix (%lf) is negative at %ld \n",diag->co[j],j);
stop_execution();
}
}
tridiag(0,0,0,x->nh,udiag,diag,udiag,r,d);
for (j=y->nl;j<=y->nh;j++) {
//d->co[j]=r->co[j]/diag->co[j];
delta_new+=r->co[j]*d->co[j];
}
norm_r0 = norm_2(r, r->nh);
while ( icnt<=icnt_max && norm_2(r, r->nh) > Fmax( tol_min , Fmin( tol_max , tol_rel*norm_r0) ) ) {
delta=delta_new;
p=0.0;
for(j=q->nl;j<=q->nh;j++) {
q->co[j]=(*function)(j,d,x0,dt,data);
p+=q->co[j]*d->co[j];
}
alpha=delta_new/p;
for(j=x->nl;j<=x->nh;j++) {
x->co[j]=x->co[j]+alpha*d->co[j];
}
delta_new=0.0;
sl=0;
for (j=y->nl;j<=y->nh;j++) {
if (icnt%MAX_ITERATIONS==0) {
y->co[j]=(*function)(j,x,x0,dt,data);
r->co[j]=b->co[j]-y->co[j];
} else {
r->co[j]=r->co[j]-alpha*q->co[j];
}
}
tridiag(0,0,0,x->nh,udiag,diag,udiag,r,sr);
for (j=y->nl;j<=y->nh;j++) {
delta_new+=sr->co[j]*r->co[j];
}
beta=delta_new/delta;
for (j=d->nl;j<=d->nh;j++) {
d->co[j]=sr->co[j]+beta*d->co[j];
}
icnt++;
//printf("i:%ld normr0:%e normr:%e\n",icnt,norm_r0,norm_2(r,r->nh));
}
//printf("norm_r:%e\n",norm_2(r,r->nh));
free_doublevector(udiag);
free_doublevector(diag);
free_doublevector(sr);
free_doublevector(r);
free_doublevector(d);
free_doublevector(q);
free_doublevector(y);
return icnt;
}
