/*-----------------------------------------------------------------------
* pvccoco-03
*
* Dans cette version : "2-opt + insertion-opt" appliqué au 14 balles avec gestion des obstacles
* Voir les fichier README.francais et COPYING
*
* Auteur : Alexandre AUPETIT (aaupetit@club-internet.fr)
* Web : http://home.alex.tuxfamily.org/pvc.html
* http://home.alex.tuxfamily.org/pvc/robotique/coconut.html
*
* Important : Ce logiciel est soumis à la licence de logiciel libre GPL
*-----------------------------------------------------------------------*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
#include "robot_math.cpp"
#define NB_BALLES 14
#define NB_OBSTACLES 2
#define NB_ITER 5
#define RAYON_ENCOMBREMENT 1.5
#define MAX_LENLINE 255
#define GAIN_MINI -0.0000000001
double pos_x[NB_BALLES+1]; // tableau des position en X des balles (en 0 : pos initiale du robot)
double pos_y[NB_BALLES+1]; // tableau des position en Y des balles (en 0 : pos initiale du robot)
double pos_x_o[NB_OBSTACLES]; // tableau des position en X des obstacles
double pos_y_o[NB_OBSTACLES]; // tableau des position en Y des obstacles
double cache_distance[NB_BALLES+1][NB_BALLES+1];
int parcours[NB_BALLES+1]; // parcours (numéros des balles) (en 0 : pos initiale du robot)
double score_parcours; // coût du parcours == distance totale
int best_parcours[NB_BALLES+1]; // meme chose en sauvegarde (meilleur parcours calculé jusqu'à présent)
double score_best_parcours;
// retourne un chiffre au hasard entre 0 et max
int hasard(int max) {
return (int)((rand()/(RAND_MAX + 1.0))*max);
}
double distance_point(int point1, int point2) {
return cache_distance[point1][point2];
}
// distance entre deux points dont les coordonnées sont définies dans pos_x et pos_y
// prend en compte les obstacles
double distance_point_calcul(int point1, int point2) {
int i;
double dist, decalage;
// distance euclidienne
dist=sqrt((pos_x[point2]-pos_x[point1])*(pos_x[point2]-pos_x[point1])
+(pos_y[point2]-pos_y[point1])*(pos_y[point2]-pos_y[point1]));
// décalage pour chaque obstacle
for (i=0; i<NB_OBSTACLES; i++) {
if (detect_collision(pos_x[point1],pos_y[point1],pos_x[point2],pos_y[point2],
pos_x_o[i], pos_y_o[i], RAYON_ENCOMBREMENT, &decalage)) {
dist+=2*decalage;
/*printf("collision : %d-%d (obstacle %d)\n", point1, point2, i);*/
}
}
return dist;
}
// pré-calcul des distances dans un tableau
void remplit_cache_distance() {
int i, j;
for (i=0;i<NB_BALLES+1;i++) {
cache_distance[i][i]=0;
for (j=i+1;j<NB_BALLES+1;j++) {
cache_distance[i][j]=distance_point_calcul(i, j);
cache_distance[j][i]=cache_distance[i][j];
}
}
}
// distance totale du parcours
// attention on prend en compte le parcours entre le robot et la première balle
// (parcours[0] à parcours[1]) mais on ne revient pas à la position initiale
// contrairement au PVC classique
double distance_parcours() {
int i;
double d=0;
for (i=1;i<NB_BALLES+1;i++) {
d+=distance_point(parcours[i-1], parcours[i]);
}
return d;
}
void copie_best_parcours() {
int i;
for (i=1;i<NB_BALLES+1;i++) {
best_parcours[i]=parcours[i];
}
score_best_parcours=score_parcours;
}
// affichage du parcours (et de son score) à l'écran
// avec aussi un message d'info
void affiche_parcours(char *message_info) {
int i;
printf("%s", message_info);
for (i=0;i<NB_BALLES+1;i++) {
printf("%d-", parcours[i]);
}
printf("(d=%lf)\n", score_parcours);
}
// affichage du meilleurs parcours (et de son score) à l'écran
// avec aussi un message d'info
void affiche_best_parcours(char *message_info) {
int i;
printf("%s", message_info);
for (i=0;i<NB_BALLES+1;i++) {
printf("%d-", best_parcours[i]);
}
printf("(d=%lf)\n", score_best_parcours);
}
// initialisation de la position des balles par lecture du fichier pos.txt
// le fichier pos_balles.txt doit contenir les positions au format :
// x_robot;y_robot
// x_balle1;y_balle1
// x_balle2;y_balle2
// x_balle3;y_balle3
// ...
// x_balle8;y_balle8
void init_pos() {
FILE *ficPos;
char sLine[MAX_LENLINE + 1];
char *sPos;
int i;
ficPos=fopen("pos_balles.txt", "rt");
if (ficPos != NULL) {
i=0;
fgets(sLine, MAX_LENLINE, ficPos);
do {
sPos = strtok(sLine, ";\n");
//printf("%s-\n",sPos);
pos_x[i] = atof(sPos);
sPos = strtok(NULL, ";\n");
//printf("%s-\n",sPos);
pos_y[i] = atof(sPos);
i++;
fgets(sLine, MAX_LENLINE, ficPos);
} while ( !(feof(ficPos)) );
/*if (i != NB_BALLES+2) {
printf("Erreur dans le nombre de balles du fichier pos_balles.txt !\n");
exit(1);
}*/
} else {
printf("Erreur d'ouverture du fichier pos_balles.txt !");
exit(1);
}
fclose(ficPos);
// obstacles
ficPos=fopen("pos_obstacles.txt", "rt");
if (ficPos != NULL) {
i=0;
fgets(sLine, MAX_LENLINE, ficPos);
do {
sPos = strtok(sLine, ";\n");
pos_x_o[i] = atof(sPos);
sPos = strtok(NULL, ";\n");
pos_y_o[i] = atof(sPos);
i++;
fgets(sLine, MAX_LENLINE, ficPos);
} while ( !feof(ficPos) );
/*if (i != NB_OBSTACLES+1) {
printf("Erreur dans le nombre d'obstacles du fichier pos_obstacles.txt !");
exit(1);
}*/
} else {
printf("Erreur d'ouverture du fichier pos_obstacles.txt !");
exit(1);
}
fclose(ficPos);
/*for (i=0; i<NB_BALLES+1;i++) {
printf("balle %d : %lf %lf\n", i, pos_x[i], pos_y[i]);
}
for (i=0; i<NB_OBSTACLES;i++) {
printf("obstacle %d : %lf %lf\n", i, pos_x_o[i], pos_y_o[i]);
}*/
}
// echange l'odre de parcours entre 2 balles
void echange_parcours(int ordre1, int ordre2) {
int inter;
inter=parcours[ordre2];
parcours[ordre2]=parcours[ordre1];
parcours[ordre1]=inter;
}
// renverse le parcours entre les balles i et j
// il s'agit de la transformation la plus simple d'un parcours
// voir graphiquement ce que ça représente
// c'est la transformation de base du 2-opt : le 2-change
void renverse_parcours(int i, int j) {
int a, b;
if (i < j) {
a = i;
b = j;
} else {
a = j;
b = i;
}
while (a < b) {
echange_parcours (a, b);
a++;
b--;
}
}
// initialise un parcours au hasard
void calcule_parcours_hasard() {
int i;
for (i=1;i<NB_BALLES+1;i++) {
parcours[i] = i;
}
int a,b;
for (i=1;i<NB_BALLES+1;i++) {
a=hasard(NB_BALLES)+1;
b=hasard(NB_BALLES)+1;
echange_parcours(a, b);
}
}
// --------------- 2-OPT --------------------------
// différence de cout du parcours si on renversait le parcours entre les balles i et j
// on "casse" 2 liens entre 2 balles et on les remplace par 2 autres
double difference_cout_2opt(int i, int j) {
if (j+1<NB_BALLES+1) {
return (distance_point(parcours[i], parcours[j+1])
+ distance_point(parcours[i-1], parcours[j])
- distance_point(parcours[i-1], parcours[i])
- distance_point(parcours[j], parcours[j+1]));
} else {
return (
distance_point(parcours[i-1], parcours[j])
- distance_point(parcours[i-1], parcours[i])
);
}
}
// calcule un parcours 2-opt, c'est à dire qu'aucune permutation
// de l'odre de parcours ne rend ce parcours plus court
void calcule_parcours_2opt() {
int i, j, modifie;
do {
modifie=0;
for (i=1;i<NB_BALLES+1;i++) {
for (j=i+1;j<NB_BALLES+1;j++) {
if (difference_cout_2opt(i, j) < 0.0) {
renverse_parcours(i, j);
modifie=1;
}
}
}
} while (modifie == 1);
}
// --------------- INSERTION-OPT --------------------------
// différence de longueur de trajet lorsqu'on isère i entre j et j+1
// attention aux conditions pour lesquelles ce gain est valide ( i+1<j ) ...
double difference_cout_insert(int i, int j) {
if (j+1<NB_BALLES+1) {
return (distance_point(parcours[i], parcours[j])
+ distance_point(parcours[i], parcours[j+1])
+ distance_point(parcours[i-1], parcours[i+1])
- distance_point(parcours[i-1], parcours[i])
- distance_point(parcours[i], parcours[i+1])
- distance_point(parcours[j], parcours[j+1]));
} else {
return (distance_point(parcours[i], parcours[j])
+ distance_point(parcours[i-1], parcours[i+1])
- distance_point(parcours[i-1], parcours[i])
- distance_point(parcours[i], parcours[i+1])
);
}
}
// différence de longueur de trajet lorsqu'on isère j entre i et i+1
double difference_cout_insert_inv(int i, int j) {
if (j+1<NB_BALLES+1) {
return (distance_point(parcours[i-1], parcours[j])
+ distance_point(parcours[i], parcours[j])
+ distance_point(parcours[j-1], parcours[j+1])
- distance_point(parcours[i-1], parcours[i])
- distance_point(parcours[j-1], parcours[j])
- distance_point(parcours[j], parcours[j+1]));
} else {
return (distance_point(parcours[i-1], parcours[j])
+ distance_point(parcours[i], parcours[j])
- distance_point(parcours[i-1], parcours[i])
- distance_point(parcours[j-1], parcours[j])
);
}
}
// insere la balle i entre j et j+1
// attention, il faut que i<j !!
void insert_parcours (int i, int j)
{
int a, b, i_temp;
a = i;
b = j;
i_temp = parcours[a];
while (a < b) {
parcours[a] = parcours[a + 1];
a++;
}
parcours[b] = i_temp;
}
// insere la balle j entre i et i+1
// attention, il faut que i<j !!
void insert_inv_parcours (int i, int j)
{
int a, b, i_temp;
a = i;
b = j;
i_temp = parcours[b];
while (b > a) {
parcours[b] = parcours[b - 1];
b--;
}
parcours[a] = i_temp;
}
// calcule un parcours insert-opt, c'est à dire qu'aucune insertion
// d'une ville à un autre endroit du parcours ne rend ce parcours plus court
void calcule_parcours_insert() {
int i, j;
for (i=1;i<NB_BALLES+1;i++) {
for (j=i+2;j<NB_BALLES+1;j++) {
if (difference_cout_insert(i, j) < 0.0) {
insert_parcours(i, j);
} else {
if (difference_cout_insert_inv(i, j) < 0.0) {
insert_inv_parcours(i, j);
}
}
}
}
}
// prcédure qui combine de manière très efficace calcule_parcours_2opt et calcule_parcours_insert
// Le parcours résultant est 2-opt et insertion-opt
void calcule_parcours_2opt_et_insert() {
int i, j, modifie;
do {
modifie=0;
//2-opt
for (i=1;i<NB_BALLES+1;i++) {
for (j=i+1;j<NB_BALLES+1;j++) {
if (difference_cout_2opt(i, j) < GAIN_MINI) {
renverse_parcours(i, j);
modifie=1;
//printf("2-opt : i=%d j=%d\n", i, j);
}
}
}
// insert-opt
// le 2-opt est prioritaire car il conduiut à de meilleures solutions,
// d'où la condition modifie==0
for (i=1;(i<NB_BALLES+1)&&(modifie==0);i++) {
for (j=i+2;(j<NB_BALLES+1)&&(modifie==0);j++) {
if (difference_cout_insert(i, j) < GAIN_MINI) {
insert_parcours(i, j);
modifie=1;
} else {
if (difference_cout_insert_inv(i, j) < GAIN_MINI) {
insert_inv_parcours(i, j);
modifie=1;
}
}
}
}
} while (modifie == 1);
}
// ----------------------------------------------------------------------------
// prog principal
// ----------------------------------------------------------------------------
int main(int argc, char *argv[]) {
int i, j, nb_essais;
srand (time (NULL));
nb_essais=1;
if (argc == 2) {
nb_essais=atoi(argv[1]);
} else {
if (argc != 1) {
printf("Erreur dans le nombre d'arguments !\n");
printf("Usage : pvccoco [Nb d'essais]\n");
exit(1);
}
}
init_pos();
for (i=0;i<nb_essais;i++) {
// pour ne pas fausser le calcul du temps unitaire,
// le remplissage du cache est refait à chaque itération
remplit_cache_distance();
for (j=0;j<NB_ITER;j++) {
calcule_parcours_hasard();
calcule_parcours_2opt_et_insert();
score_parcours=distance_parcours();
affiche_parcours(" itération : ");
if ((j==0) || (score_parcours < score_best_parcours)) {
copie_best_parcours();
}
}
affiche_best_parcours("Chemin trouvé : ");
}
return 0;
}