glm::ivec2 MapViewer::pixel_to_tile(glm::ivec2 pixel_location) {
float scale(Engine::get_actual_tile_size());
glm::vec2 display_position(get_display_x(), get_display_y());
glm::vec2 float_result = (glm::vec2(pixel_location) / scale) + display_position;
return (glm::ivec2)float_result;
}
// Disp a specific screen
void DisplayManager::display() {
lcd.clear(); lcd.setCursor(0, 0);
if (display_mode == -2) {
display_menu();
return;
}
switch (display_mode) {
case DATETIME:
display_datetime();
break;
case POSITION:
display_position();
break;
case ALTITUDE:
display_altitude();
break;
case BATTERYM:
display_battery();
break;
default:
lcd.print(satellites);
lcd.setCursor(0, 1);
lcd.print(iterations);
return;
}
}
glm::ivec2 MapViewer::tile_to_pixel(glm::vec2 tile_location) {
float scale(Engine::get_actual_tile_size());
glm::vec2 display_position(get_display_x(), get_display_y());
// Screen offset is reduced by the offset of the display
return (tile_location - display_position) * scale;
}
/*------------------------------------------------------------------- DISPLAY_SOLUTION */
void display_solution(struct particle p)
{
if (p.rank<0)
{
printf("\n Extra particle");
}
display_position(p,1);
}
/*------------------------------------------------------------------- DISPLAY_SWARM */
void display_swarm(struct swarm sw)
{
int i,i_best;
i_best=0;
printf("\n SWARM");
for (i=0; i<sw.size; i++) /* For each "particle" */
{
display_position(sw.p[i],1);
if (sw.p[i].f<sw.p[i_best].f) i_best=i;
}
printf("\n Best particle: %i, f value %.1f",i_best+1,sw.p[i_best].f);
printf("\n");
}
//============================================================== ARC_AROUND
struct particle arc_around(struct swarm sw, struct particle p)
{ // Build a tour, asking opinion of other particles for each arc
double big_value;
int candidate[Nmax];
double candidate_penalty[Nmax];
int candidate_nb;
double c_max;
int i;
int j;
int k;
//int m;
int node1,node2;
double penalty;
struct particle pt;
double r;
int rank;
int used[Nmax]= {0};
big_value=G.N*G.l_max;
pt=p;
node1=0; // The new tour begins in 0
rank=1;
used[node1]=1;
next_node:
candidate_nb=0;
for (node2=0; node2<G.N; node2++) // For each possible new node ...
{
if (used[node2]>0) continue;
if(rank==G.N-1) goto set_node; // Last node => no choice, anyway
penalty=0;
for (j=0; j<sw.size; j++) // ... ask opinions
{
for (k=0; k<G.N-1; k++) // Find the arc in the current particle, if exists
if (sw.p[k].best_x.s[k]==node1 && sw.p[k].best_x.s[k+1]==node2 )
{
penalty=sw.p[k].best_f;
goto next_opinions;
}
// Can't find it in the current particle
penalty=big_value;
next_opinions:
;
}
// Mean value
candidate_penalty[candidate_nb]=penalty/sw.size;
candidate[candidate_nb]=node2;
candidate_nb=candidate_nb+1;
} // next node2
// Choose the new node at random, according to penalties
c_max=0;
for (i=0; i<candidate_nb; i++) c_max=c_max+ candidate_penalty[i];
for (i=0; i<candidate_nb; i++) candidate_penalty[i]=1-candidate_penalty[i]/c_max; //Proba
for (i=1; i<candidate_nb; i++) candidate_penalty[i]=candidate_penalty[i-1]+candidate_penalty[i]; // Cumulate
r=alea_float(0,candidate_nb-1); // random choice
//printf("\n %i: ",candidate_nb);for (i=0;i<candidate_nb;i++) printf(" %f", candidate_penalty[i]);
for (i=0; i<candidate_nb; i++)
{
if(r<candidate_penalty[i])
{
node2=candidate[i];
goto set_node;
}
}
set_node:
// printf("\n r %f, i %i node2 %i",r,i,node2);
pt.x.s[rank]=node2; // Add the node
used[node2]=1; // remember it is already used
rank=rank+1;
if (rank<G.N) goto next_node;
pt.x.s[G.N]=pt.x.s[0]; // Complete the tour
pt.f=f(pt,-1,-1); // evaluate
if (pt.f<pt.best_f) // eventually update the best previous position
{
pt.best_x=pt.x;
pt.best_f=pt.f;
pt.best_time=time;
}
if (trace>1)
{
display_position(pt,1);
display_position(pt,2);
}
BB_update(pt); // Update the blackboard
return pt;
}
int main()
{
FILE *f_trace;
FILE *f_graph; // File containing the graph
FILE *f_run; // To save swarm positions, for each iteration
int auto_move_type;
struct particle best_hood; // Best particle in the neighbourhood
double best_best_f;
struct coeff coeff;
double eps_max,eps_mean,eps_min;
int hamilt;
int i;
int improv,improv_best;
//int iter,iter_min;
//struct graph G2;
char graph[80]; // Graph file name
int j;
int k;
// int m;
//float level;
//int loop_auto_move;
float max_eval;
//int max_iter;
double mean_eval;
float min_tour;
int move4;
struct particle new_p;
int norm_v;
int n_success;
//int parallel;
int run,run_max;
int scanR;
int swarm_size;
double zzz,zzz2;
/*----------------- SOME PARAMETERS -------------- */
#include "param.c"
f_trace=fopen("trace.txt","w");
if (save==0) goto graph;
/*-------------------------- */
printf("\nChoose the file to save the run, please");
scanR=scanf("%s",graph);
f_run=fopen(graph,"w");
/*-------------------------- */
graph:
/* Read the valuated graph (Note: value 0 <=> no arc) */
printf("\nFile name for the graph, please: ");
scanR=scanf("%s",graph);
//printf("\nMultiply all arc values by: ");
//scanf("%f",&integer_coeff) ;
integer_coeff=1;
f_graph=fopen(graph,"r");
G=read_graph(f_graph,trace);
//display_graph(G);
/* // Save the graph
for (i=0;i<G.N;i++)
{
fprintf(f_trace,"\n");
for (j=0;j<G.N;j++) fprintf(f_trace,"%.0f ",G.v[i][j]);
}
return EXIT_SUCCESS;
*/
printf("\n Just looking for Hamilton cycles ? (y/n): ");
scanR=scanf("%s",answer);
if (answer[0]=='y')
{
G=TSP_to_Hamilton(G);
return EXIT_SUCCESS;
}
G=graph_min_max(G); /* Compute the max and the min arc values, and the number of edges */
min_tour=G.N*G.l_min;
printf("\n Target ? (suggestion %.0f) ",min_tour);
scanR=scanf("%f",&target);
/*------------------------------ */
//graph_study:
hamilt=check_Hamilton_cycle(G);
if (hamilt==0) printf("\n\n WARNING. There is NO Hamiltonian cycle. I look for a Hamiltonian path\n");
if(hamilt==1 || hamilt==2)printf("\n\n LUCKY YOU. There IS at least one Hamiltonian cycle. I look for the best one\n");
if (hamilt==3) printf("\n I don't know whether there is a Hamiltonian cycle or not. Let's try");
//parameters:
printf("\n\n Default parameters");
//swarm_size=G.N; // Just a rule of thumb
//zzz=0; for (i=2;i<G.N;i++) zzz=zzz+log(i);
//swarm_size=2.46*zzz-55; // Another rule of thumb (valid only if the result is >0
swarm_size=2*G.N+1; // Another rule of thumb
//swarm_size=400;
printf("\nSwarm size ......... %i",swarm_size);
swarm_size=MAX(1,MIN(swarm_size,Max_size));
sw1.size=swarm_size;
// see param.c
printf("\n Init option................ %i",init_option);
printf("\n Neighbourhood size.. %i ",hood);
printf("\nHood_type .............%i",hood_type);
printf("\nMove_type ............. %i",move[0]);
if(move[0]<=5)
{
printf("\nKappa value ...... %.2f",kappa) ;
printf("\nPhi value ............%.2f",phi);
}
printf("\nAuto_move_type ....%i",move[1]);
printf("\nSplice_around the best %i",move[2]);
printf("\nSplice_around more.... %i",move[3]);
printf("\nRebuild method .......... %i",move[4]);
//same_best_thresh=G.N/(2*hood); //Just a rule of thumb
size_max=G.N; // Max size for splice_around. Just a rule of thumb
printf("\n Do you want to modify them? (y/n): ") ;
scanR=scanf("%s",answer);
if (answer[0]=='n') goto end_param;
// -----------------------------------Ask for parameters
printf("\n Swarm size? (max = %i) ",Max_size);
scanR=scanf("%i",&swarm_size);
swarm_size=MAX(1,MIN(swarm_size,Max_size));
sw1.size=swarm_size;
printf("\n Init option? (suggested 1 or 2): ");
scanR=scanf("%i",&init_option);
printf("\n Neighbourhood size? (max = %i) ",sw1.size);
scanR=scanf("%i",&hood);
hood=MAX(1,MIN(hood,sw1.size));
printf("\n Hood type? (0 = social (quick) 1 = physical (long)\n (suggestion: %i): ",hood_type);
scanR=scanf(" %i",&hood_type);
printf("\n Move type? (1 to 10) (suggestion: %i): ",move[0]);
scanR=scanf("%i",&move[0]);
if(move[0]<=5)
{
printf("\n kappa value? (suggestion: %.2f): ",kappa); // Constriction coefficients. See move_towards
scanR=scanf("%f",&kappa);
printf("\n phi value? (suggestion: %.2f): ",phi);
scanR=scanf("%f",&phi);
}
printf("\n Auto-move type? (0 to 6) (suggestion: %i): ",move[1]);
scanR=scanf("%i",&move[1]);
printf("\nSplice_around the best?(suggestion %i)",move[2]);
scanR=scanf("%i",&move[2]);
printf("\nSplice_around more? (suggestion %i)",move[3]);
scanR=scanf("%i",&move[3]);
printf("\nRebuild method? (suggestion %i)",move[4]);
scanR=scanf("%i",&move[4]);
end_param:
//-------------------------------- Iterations
printf("\n Max tour evaluations? (0 => end) ");
scanR=scanf("%f",&max_eval);
if (max_eval==0) goto the_end;
printf("\n Trace level ? (0,1,2,3,4) ");
scanR=scanf("%i",&trace);
/* ======================================================== HERE IS THE ALGORITHM */
printf("\n How many times? ");
scanR=scanf("%i",&run_max);
if (run_max==0 ) goto the_end;
run=1;
n_success=0;
eps_min=10000*target;
eps_max=0;
eps_mean=0;
mean_eval=0;
if (move[4]>=3) // Initialize the blackboard
{
for (i=0; i<G.N; i++) for (j=0; j<G.N; j++) for (k=0; k<2; k++) BB[k].v[i][j]=0;
}
loop_run:
printf("\n ___________________________ RUN %i",run);
eval_f=0;
time=0; // Current time step
splice_time=0; // Last time splice_around has been used
// Initialize the swarm
sw1=init_swarm(swarm_size,G,trace);
printf("\nAfter init: eval %.0f value %f for particle %i",eval_f, best_best.best_f,sw1.rank_best);
if (best_best.best_f<=target) goto success;// Success
if (save!=0) save_swarm(f_run,sw1); /* Save the run as text file */
if(move[0]<=5 && move[0]!=0)
coeff=convergence_case(kappa,phi); // Coeffs for non spherical methods
moves:
time=time+1;
printf("\n total velocity %.0f",tot_vel(sw1));
if (move[0]>0)
{
//----------------------------------------------------------------------------- Normal move
printf("\nEval %.0f. Normal move %i",eval_f,move[0]);
if (eval_f>=max_eval) goto end_max_eval;
best_best_f=best_best.best_f; // Before the move
for (i=0; i<sw1.size; i++)
{
// find the best in the neighbourhood (or the local queen)
best_hood=best_neighbour(sw1,sw1.p[i],hood,hood_type,monotony,equiv_v,trace);
// M O V E
sw1.p[i]=move_towards(sw1.p[i],best_hood,coeff,move[0],explicit_vel,conv_case,equiv_v,trace);
if (sw1.p[i].best_f<best_best.best_f)// Check best of the best after the move
{
best_best=sw1.p[i];
sw1.rank_best=i;
printf("\neval %.0f value %f",eval_f, best_best.best_f);
display_position(best_best,1);
}
if (best_best.best_f<=target) goto success;// Success
if (best_best.best_f<best_best_f) goto moves; // Loop as soon as there is a global improvement
} // next i for normal move
}
if (move[1]>0)
{
//------------------------------------------------------------- If no improvement, auto_move
printf(" /auto_move %i",move[1]);
auto_move_type=move[1];
// auto_move:
for (i=0; i<sw1.size; i++)
{
sw1.p[i]= auto_move(sw1.p[i],auto_move_type,0,0,trace);
if (sw1.p[i].best_f<best_best.best_f)
{
best_best=sw1.p[i];
sw1.rank_best=i;
if (best_best.best_f<=target) goto success;// Success
printf("\neval %.0f value %f",eval_f, best_best.best_f);
display_position(best_best,1);
goto moves;
}
if (eval_f>=max_eval) goto end_max_eval;
} // next i for auto_move
}
if(move[2]>0)
{
// --------------------------------------------------- If still no improvement, splice_around best particle
printf(" /splice_around, best particle");
i= sw1.rank_best;
sw1.p[i]= splice_around( sw1, sw1.p[i], hood_type,hood);
if (sw1.p[i].best_f<best_best.best_f)// Best of the best after the move
{
best_best=sw1.p[i];
if (best_best.best_f<=target) goto success;// Success
printf("\neval %.0f value %f",eval_f, best_best.best_f);
goto moves;
}
}
if(move[3]>0)
{
// --------------------------------------------------- If still no improvement, splice_around more particles
printf(" /splice_around, more, option %i",splice_around_option);
improv=0;
improv_best=0;
splice_time=time;
for (i=0; i<sw1.size; i++) // For each particle
{
zzz= sw1.p[i].f;
zzz2= sw1.p[i].best_f;
sw1.p[i]= splice_around( sw1, sw1.p[i], hood_type,hood);
if (sw1.p[i].f<zzz) improv=improv+1;
if (sw1.p[i].best_f<zzz2) improv_best=improv_best+1;
if (sw1.p[i].best_f<best_best.best_f)// Best of the best after the move
{
best_best=sw1.p[i];
sw1.rank_best=i;
if (best_best.best_f<=target) goto success;// Success
printf("\neval %.0f value %f",eval_f, best_best.best_f);
goto moves;
}
if (eval_f>=max_eval) goto end_max_eval;
} // next i for splice_around
// if (improv_best>sw1.size/2) goto moves;
}
if (move[4]==9) move4=alea(1,3);
else move4=move[4];
if(move4>0)
{
//--------------------------------- If still no improvement, rebuild tours
if (move4==1) printf(" /rebuild, random");
if (move4==2) printf(" /rebuild, arc_around");
if (move4==3) printf(" /rebuild, blackboard");
for (i=0; i<sw1.size; i++)
{
switch( move4)
{
case 1:
norm_v=alea(1,G.N);
sw1.p[i].v=alea_velocity(norm_v); // random velocity
sw1.p[i]=pos_plus_vel(sw1.p[i],sw1.p[i].v,0,0,0); // => random new position
break;
case 2:
new_p=arc_around(sw1,sw1.p[i]);
sw1.p[i].v=coeff_pos_minus_pos(new_p,sw1.p[i],monotony,0,equiv_v,trace);
sw1.p[i]=new_p;
break;
case 3:
new_p=BB_use(sw1.p[i]);
sw1.p[i].v=coeff_pos_minus_pos(new_p,sw1.p[i],monotony,0,equiv_v,trace);
sw1.p[i]=new_p;
break;
}
if (sw1.p[i].best_f<best_best.best_f)/* Best of the best after the move */
{
best_best=sw1.p[i];
if (best_best.best_f<=target) goto success;// Success
printf("\neval %.0f value %f",eval_f, best_best.best_f);
goto moves;
}
if (eval_f>=max_eval) goto end_max_eval;
}
}
// Primitive rehope
printf("\n Primitive rehope");
for (i=0; i<sw1.size; i++)
{
sw1.p[i].v=alea_velocity(G.N);
sw1.p[i]= pos_plus_vel(sw1.p[i],sw1.p[i].v,0,0,0);
}
goto moves;
//-----------------------------------
// end_move:
if (save!=0) save_swarm(f_run,sw1);
if (trace>1) display_swarm(sw1);
printf("\n Eval %.0f",eval_f);
if (trace>0) display_position(best_best,2);
goto moves;
end_max_eval:
printf("\n\n Stop at max eval %.0f",eval_f);
goto end;
success:
n_success=n_success+1;
printf("\n SUCCESS");
end:
// Give the best position (sequence) found
printf("\n Best solution found after %.0f evaluations",eval_f);
if (best_best.best_x.s[0]!=0) best_best.best_x=rotate(best_best.best_x,0);
display_position(best_best,2);
// Check if no error
zzz=f(best_best,-1,-1);
if (zzz!=best_best.best_f)
{
printf("\n ERROR. The true f value is in fact %f ",zzz);
}
zzz=best_best.best_f;
if (zzz<eps_min) eps_min= zzz;
if (zzz>eps_max) eps_max= zzz;
eps_mean=eps_mean+zzz;
mean_eval=mean_eval+eval_f;
if (save!=0) save_swarm(f_run,sw1);
save_swarm(f_trace,sw1);
if (trace>1) display_swarm(sw1);
run=run+1;
if (run<=run_max) goto loop_run;
else
{
printf("\n With swarm size=%i, max eval=%.0f => %i successes/%i. Rate=%.3f ",swarm_size,max_eval,n_success,run_max,(float)n_success/run_max);
printf("\n Mean %f [%f, %f]",eps_mean/run_max,eps_min,eps_max);
mean_eval=mean_eval/run_max;
printf("\n Mean eval %f",mean_eval);
goto the_end;
}
//error_end:
printf("\n Sorry, I give up");
the_end:
return EXIT_SUCCESS;
}
