566 lines
16 KiB
C
566 lines
16 KiB
C
#include "vehicle.h"
|
|
|
|
#define NB_ACTION 3
|
|
#define NB_SENSORS 3
|
|
|
|
//#define LEFT 0
|
|
//#define CENTER 1
|
|
//#define RIGHT 2
|
|
|
|
#define LIMIT_DISTANCE ((float)1)/50
|
|
|
|
#define REWARD_STOP -1000
|
|
#define REWARD_CONTINUE 100
|
|
#define THRESHOLD_REWARD 10000
|
|
|
|
|
|
struct game_status * create_game_status(){
|
|
struct game_status * status = malloc(sizeof(struct game_status));
|
|
status->q_state = 0;
|
|
status->reward = 0;
|
|
status->cumulative_reward = 0;
|
|
status->done = false;
|
|
}
|
|
|
|
coordinate * create_coordinate(size_t dim_size){
|
|
dimension * dim = create_dim(1);
|
|
dim->perm[0] = dim_size;
|
|
updateRankDim(dim);
|
|
coordinate * ret_coord = CREATE_TENSOR_TYPE_FLOAT(dim);
|
|
//struct coordinate * ret_coord = malloc(sizeof(struct coordinate));
|
|
//ret_coord->dimension_size = dim_size;
|
|
//ret_coord->x = malloc(dim_size * sizeof(float));
|
|
|
|
return ret_coord;
|
|
}
|
|
struct blocks * create_blocks(size_t nb_blocks, size_t dim_size){
|
|
struct blocks * ret_blocks = malloc(sizeof(struct blocks));
|
|
ret_blocks->nb_blocks = nb_blocks;
|
|
ret_blocks->dimension_size = dim_size;
|
|
ret_blocks->lower_bound_block = malloc(nb_blocks * sizeof(coordinate *));
|
|
ret_blocks->upper_bound_block = malloc(nb_blocks * sizeof(coordinate *));
|
|
ret_blocks->bounds_all_blocks = NULL;
|
|
ret_blocks->all_updated = false;
|
|
ret_blocks->marker = malloc(nb_blocks * sizeof(bool));
|
|
|
|
for(size_t i=0; i<nb_blocks; ++i){
|
|
ret_blocks->lower_bound_block[i] = create_coordinate(dim_size);
|
|
ret_blocks->upper_bound_block[i] = create_coordinate(dim_size);
|
|
ret_blocks->marker[i] = false;
|
|
}
|
|
|
|
return ret_blocks;
|
|
}
|
|
|
|
sensors * create_sensors(size_t nb_xs){
|
|
dimension * dim = create_dim(1);
|
|
dim->perm[0] = nb_xs;
|
|
sensors * ret_sensors = CREATE_TENSOR_TYPE_FLOAT(dim);//malloc(sizeof(struct sensors));
|
|
//ret_sensors->nb_xs = nb_xs;
|
|
//ret_sensors->x = malloc(nb_xs * sizeof(float));
|
|
|
|
return ret_sensors;
|
|
}
|
|
|
|
struct vehicle * create_vehicle(struct blocks *path){
|
|
struct vehicle * ret_vehicle = malloc(sizeof(struct vehicle));
|
|
|
|
ret_vehicle->coord = create_coordinate(2);
|
|
ret_vehicle->sensor = create_sensors(NB_SENSORS);
|
|
ret_vehicle->old_sensor = create_sensors(NB_SENSORS);
|
|
ret_vehicle->path = path;
|
|
|
|
ret_vehicle->status = create_game_status();
|
|
|
|
reset(ret_vehicle);
|
|
|
|
return ret_vehicle;
|
|
|
|
}
|
|
|
|
void free_game_status(struct game_status *status){
|
|
free(status);
|
|
}
|
|
|
|
void free_coordinate(coordinate *coord){
|
|
//free(coord->x);
|
|
//free(coord);
|
|
free_tensor_TYPE_FLOAT(coord);
|
|
}
|
|
|
|
void free_blocks(struct blocks *blk){
|
|
for(size_t i=0; i<blk->nb_blocks; ++i){
|
|
free_coordinate(blk->lower_bound_block[i]);
|
|
free_coordinate(blk->upper_bound_block[i]);
|
|
}
|
|
free(blk->lower_bound_block);
|
|
free(blk->upper_bound_block);
|
|
if(blk->bounds_all_blocks != NULL){
|
|
free_coordinate(blk->bounds_all_blocks[0]);
|
|
free_coordinate(blk->bounds_all_blocks[1]);
|
|
free(blk->bounds_all_blocks);
|
|
}
|
|
free(blk->marker);
|
|
free(blk);
|
|
}
|
|
|
|
void free_sensors(sensors *snsr){
|
|
//free(snsr->x);
|
|
//free(snsr);
|
|
free_tensor_TYPE_FLOAT(snsr);
|
|
}
|
|
|
|
void free_vehicle(struct vehicle * vhcl){
|
|
free_coordinate(vhcl->coord);
|
|
free_blocks(vhcl->path);
|
|
free_sensors(vhcl->sensor);
|
|
free_sensors(vhcl->old_sensor);
|
|
free_game_status(vhcl->status);
|
|
|
|
free(vhcl);
|
|
}
|
|
|
|
|
|
float scalar_product(coordinate *coord1, coordinate *coord2){
|
|
/* size_t dimension_size = coord1->dimension_size;
|
|
if(coord1->dimension_size > coord2->dimension_size )
|
|
dimension_size = coord2->dimension_size;
|
|
|
|
float scalar = 0;
|
|
for(size_t i=0; i<dimension_size; ++i){
|
|
scalar += (coord1->x[i] * coord2->x[i]);
|
|
}
|
|
|
|
return scalar;
|
|
*/
|
|
return scalarProduct_0_TYPE_FLOAT(coord1, coord2);
|
|
}
|
|
|
|
float vector_norm2(coordinate * coord){
|
|
float scalar = scalar_product(coord, coord);
|
|
return (float)sqrt(scalar);
|
|
}
|
|
|
|
bool is_in_block_index(struct blocks *blk, size_t index, coordinate *coord){
|
|
if(blk->dimension_size != coord->dim->rank) return false;
|
|
for(size_t j=0; j<blk->dimension_size; ++j){
|
|
if( ((blk->lower_bound_block[index])->x[j] > coord->x[j] ) ||
|
|
((blk->upper_bound_block[index])->x[j] < coord->x[j] ) ){
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
int is_in_blocks(struct blocks *blk, coordinate *coord){
|
|
if(blk->dimension_size != coord->dim->rank) return 0;
|
|
size_t count_in = 0;
|
|
for(size_t i=0; i<blk->nb_blocks; ++i){
|
|
count_in = 0;
|
|
for(size_t j=0; j<blk->dimension_size; ++j){
|
|
if( ((blk->lower_bound_block[i])->x[j] <= coord->x[j] ) &&
|
|
((blk->upper_bound_block[i])->x[j] >= coord->x[j] ) ){
|
|
++count_in;
|
|
}
|
|
}
|
|
if(count_in == blk->dimension_size )
|
|
return (i+1);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void printCoordinate(coordinate *coord, char *msg){
|
|
for(size_t i=0; i<coord->dim->rank; ++i){
|
|
printf(" %f,", coord->x[i]);
|
|
}
|
|
printf("{%s} [ %ld ]\n", msg, coord->dim->rank);
|
|
}
|
|
|
|
void copy_coordinate(coordinate *coord, float *x){
|
|
for(size_t i=0; i<coord->dim->rank; ++i){
|
|
coord->x[i] = x[i];
|
|
}
|
|
}
|
|
|
|
coordinate ** bounds_limits_blocks(struct blocks *blk){ /* min x , max y */
|
|
|
|
coordinate **bounds_coord = malloc(2 * sizeof( coordinate*)) ;
|
|
bounds_coord[0] = create_coordinate(blk->dimension_size);
|
|
bounds_coord[1] = create_coordinate(blk->dimension_size);
|
|
|
|
copy_coordinate(bounds_coord[0], blk->lower_bound_block[0]->x);
|
|
copy_coordinate(bounds_coord[1], blk->upper_bound_block[1]->x);
|
|
|
|
for(size_t i=1; i<blk->nb_blocks; ++i){
|
|
for(size_t j=0; j<blk->dimension_size; ++j){
|
|
bounds_coord[0]->x[j] = MIN(bounds_coord[0]->x[j], blk->lower_bound_block[i]->x[j]);
|
|
bounds_coord[0]->x[j] = MIN(bounds_coord[0]->x[j], blk->upper_bound_block[i]->x[j]);
|
|
bounds_coord[1]->x[j] = MAX(bounds_coord[1]->x[j], blk->lower_bound_block[i]->x[j]);
|
|
bounds_coord[1]->x[j] = MAX(bounds_coord[1]->x[j], blk->upper_bound_block[i]->x[j]);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
return bounds_coord;
|
|
|
|
}
|
|
|
|
void update_bounds_limits_blocks(struct blocks * blk){
|
|
if(blk->all_updated == false){
|
|
blk->all_updated = true;
|
|
|
|
if(blk->bounds_all_blocks != NULL){
|
|
free_coordinate(blk->bounds_all_blocks[0]);
|
|
free_coordinate(blk->bounds_all_blocks[1]);
|
|
free(blk->bounds_all_blocks);
|
|
}
|
|
blk->bounds_all_blocks = bounds_limits_blocks(blk);
|
|
}
|
|
}
|
|
|
|
void print2D_blocks(struct blocks *blk, float scale_x, float scale_y, char pad){
|
|
if(blk->dimension_size == 2){
|
|
//struct coordinate ** bounds_coord = bounds_limits_blocks(blk);
|
|
update_bounds_limits_blocks(blk);
|
|
coordinate ** bounds_coord = blk->bounds_all_blocks;
|
|
|
|
for(int i=0; i<(blk->dimension_size); ++i){
|
|
printf(" x[%d]: %f <= %f \n",i, bounds_coord[0]->x[i],bounds_coord[1]->x[i]);
|
|
}
|
|
|
|
coordinate * coord = create_coordinate(2);
|
|
// int offset_space = 0;
|
|
// int offset = 2;
|
|
//for(coord->x[1] = bounds_coord[0]->x[1]; coord->x[1] < bounds_coord[1]->x[1]; coord->x[1]+=scale_y )
|
|
for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > bounds_coord[0]->x[1]; coord->x[1]-=scale_y )
|
|
//for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > 0; coord->x[1]-=scale_y )
|
|
{
|
|
// printf("%*c",offset_space,' ');
|
|
// offset_space += offset;
|
|
for(coord->x[0] = bounds_coord[0]->x[0]; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x )
|
|
//for(coord->x[0] = 0; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x )
|
|
{
|
|
/*if(is_in_blocks(blk, coord))
|
|
printf("%c",pad);
|
|
*/
|
|
int in = is_in_blocks(blk,coord);
|
|
if(in)
|
|
printf("%d",in);
|
|
else
|
|
printf(" ");
|
|
}
|
|
printf("\n");
|
|
}
|
|
free_coordinate(coord);
|
|
|
|
// free_coordinate(bounds_coord[0]);
|
|
// free_coordinate(bounds_coord[1]);
|
|
// free(bounds_coord);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
struct blocks * block_neighbord_Point(coordinate *coord, float *radius ){
|
|
struct blocks * blk = create_blocks(1, coord->dim->rank);
|
|
for(size_t i=0; i<blk->dimension_size; ++i){
|
|
blk->lower_bound_block[0]->x[i] = coord->x[i]-radius[i] ;
|
|
blk->upper_bound_block[0]->x[i] = coord->x[i]+radius[i] ;
|
|
}
|
|
|
|
return blk;
|
|
}
|
|
|
|
void print2D_blocks_withPoint(struct blocks *blk, float scale_x, float scale_y, char pad, coordinate *coordPoint){
|
|
if(blk->dimension_size == 2){
|
|
//struct coordinate ** bounds_coord = bounds_limits_blocks(blk);
|
|
update_bounds_limits_blocks(blk);
|
|
coordinate ** bounds_coord = blk->bounds_all_blocks;
|
|
|
|
coordinate * coord = create_coordinate(2);
|
|
|
|
float *radius = malloc(2 * sizeof(float));
|
|
//radius[0] = MIN(scale_x, scale_y);
|
|
//radius[1] = radius[0];
|
|
radius[0]=scale_x;
|
|
radius[1]=scale_y;
|
|
|
|
struct blocks * blk_point = block_neighbord_Point(coordPoint, radius);
|
|
//for(coord->x[1] = bounds_coord[0]->x[1]; coord->x[1] < bounds_coord[1]->x[1]; coord->x[1]+=scale_y )
|
|
for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > bounds_coord[0]->x[1]; coord->x[1]-=scale_y ){
|
|
for(coord->x[0] = bounds_coord[0]->x[0]; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x ){
|
|
if(is_in_blocks(blk_point, coord))
|
|
printf("\033[0;31m"); // red
|
|
if(is_in_blocks(blk, coord))
|
|
printf("%c",pad);
|
|
/*int in = is_in_blocks(blk,coord);
|
|
if(in)
|
|
printf("%d",in);
|
|
*/
|
|
else
|
|
printf(" ");
|
|
//printf("\033[37;01m"); // white gras
|
|
printf("\033[0;37m"); // white
|
|
//printf("\033[0;30m"); // black
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
|
|
free_coordinate(coord);
|
|
|
|
free_blocks(blk_point);
|
|
free(radius);
|
|
}
|
|
}
|
|
|
|
void print2D_blocks_indexOne_withPoint(struct blocks *blk, float scale_x, float scale_y, coordinate *coordPoint){
|
|
if(blk->dimension_size == 2){
|
|
update_bounds_limits_blocks(blk);
|
|
coordinate ** bounds_coord = blk->bounds_all_blocks;
|
|
|
|
coordinate * coord = create_coordinate(2);
|
|
|
|
float *radius = malloc(2 * sizeof(float));
|
|
radius[0]=scale_x;
|
|
radius[1]=scale_y;
|
|
|
|
struct blocks * blk_point = block_neighbord_Point(coordPoint, radius);
|
|
for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > bounds_coord[0]->x[1]; coord->x[1]-=scale_y ){
|
|
for(coord->x[0] = bounds_coord[0]->x[0]; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x ){
|
|
if(is_in_blocks(blk_point, coord))
|
|
printf("\033[0;31m"); // red
|
|
int in = is_in_blocks(blk,coord);
|
|
if(in)
|
|
printf("%d",in);
|
|
else
|
|
printf("."); //printf(" ");
|
|
printf("\033[0;37m"); // white
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
|
|
free_coordinate(coord);
|
|
|
|
free_blocks(blk_point);
|
|
free(radius);
|
|
}
|
|
|
|
}
|
|
|
|
void goto_xy(int x, int y)
|
|
{
|
|
printf("%c[%d;%df", 0x1B, y, x);
|
|
}
|
|
|
|
|
|
static struct winsize w;
|
|
|
|
void init_win(){
|
|
ioctl(1, TIOCGWINSZ, &w);
|
|
}
|
|
|
|
void print_vehicle_n_path(struct vehicle *v, float scale_x, float scale_y){
|
|
static bool first = true;
|
|
if(first){
|
|
first = false;
|
|
init_win();
|
|
char pad[w.ws_col+1];
|
|
int i=0;
|
|
for(i=0; i<w.ws_col+1; ++i) pad[i]=' ';
|
|
pad[i]='\0';
|
|
for(i=0; i<w.ws_row / 2 ; ++i) printf("%s\n",pad);;
|
|
|
|
}
|
|
//goto_xy(0,w.ws_row - lines);
|
|
goto_xy(0,0);
|
|
//printf("lines : %d , row : %d : diff: %d\n",lines, w.ws_row , w.ws_row - lines);
|
|
print2D_blocks_indexOne_withPoint(v->path, scale_x, scale_y, v->coord);
|
|
//printf("lines print : %d\n",lines);
|
|
printf("\nlog : %s \n",v->status->log);
|
|
}
|
|
|
|
void move_vehicle(struct vehicle *v){
|
|
v->coord->x[0] += v->speed * cos(v->direction * M_PI / 180);
|
|
v->coord->x[1] += v->speed * sin(v->direction * M_PI / 180);
|
|
}
|
|
|
|
float distance2_coordinate(coordinate *c0, coordinate *c1){
|
|
if(c0->dim->rank != c1->dim->rank) return 0;
|
|
float d=0, tmp;
|
|
for(size_t i=0; i<c0->dim->rank; ++i){
|
|
tmp = (c0->x[i] - c1->x[i]);
|
|
d += tmp * tmp;
|
|
}
|
|
return sqrt(d);
|
|
}
|
|
|
|
#define SENSOR_VALUE_CALCULATE(position, deviation)\
|
|
direction_radian = (v->direction + deviation) * M_PI / 180;\
|
|
while( is_in_blocks(v->path, diStep_sensor )){\
|
|
diStep_sensor->x[0] += step_sensor * cos(direction_radian);\
|
|
diStep_sensor->x[1] += step_sensor * sin(direction_radian);\
|
|
}\
|
|
v->sensor->x[position] = (MIN(49,(distance2_coordinate(diStep_sensor, v->coord)/5))) ;\
|
|
|
|
//v->sensor->x[position] = (MIN(49,(distance2_coordinate(diStep_sensor, v->coord)))) / 50;\
|
|
//v->sensor->x[position] = (MIN(49,(int)(distance2_coordinate(diStep_sensor, v->coord)/10))) / 50;\
|
|
|
|
void read_sensor(struct vehicle *v){
|
|
copy_tensor_TYPE_FLOAT(v->old_sensor, v->sensor);
|
|
float step_sensor = ((float)1)/SUBDIVISION;
|
|
coordinate * diStep_sensor = create_coordinate(2);
|
|
copy_coordinate(diStep_sensor, v->coord->x);
|
|
|
|
// count the number of step until we go out of the path = distance
|
|
// center sensor
|
|
float direction_radian ;
|
|
SENSOR_VALUE_CALCULATE(CENTER,0);
|
|
|
|
copy_coordinate(diStep_sensor, v->coord->x);
|
|
// right sensor
|
|
SENSOR_VALUE_CALCULATE(RIGHT,45);
|
|
|
|
copy_coordinate(diStep_sensor, v->coord->x);
|
|
// left sensor
|
|
SENSOR_VALUE_CALCULATE(LEFT, -45);
|
|
|
|
free_coordinate(diStep_sensor);
|
|
|
|
}
|
|
|
|
#define ADD_CHAR_LOG(status, c)\
|
|
do{\
|
|
if(status->cur_log >= LOG_LENTH){\
|
|
status->cur_log = 0;\
|
|
}\
|
|
status->log[(status->cur_log)++]=c;\
|
|
}while(0); \
|
|
\
|
|
|
|
|
|
#define ADD_CHAR_LOG_ENDED(status, c)\
|
|
do{\
|
|
if(status->cur_log >= LOG_LENTH){\
|
|
status->cur_log = 0;\
|
|
}\
|
|
status->log[(status->cur_log)++]=c;\
|
|
status->log[(status->cur_log)]='\0';\
|
|
}while(0); \
|
|
\
|
|
|
|
void add_string_log_M(struct game_status *status, char *str ){
|
|
size_t i;
|
|
for(i=0; i < strlen(str) - 1; ++i){
|
|
ADD_CHAR_LOG(status, str[i]);
|
|
}
|
|
ADD_CHAR_LOG_ENDED(status, str[i]);
|
|
}
|
|
|
|
|
|
void add_string_log(struct game_status *status, char *str ){
|
|
size_t i;
|
|
for(i=0; i < strlen(str) - 1; ++i){
|
|
if(status->cur_log >= LOG_LENTH){
|
|
status->cur_log = 0;
|
|
}
|
|
status->log[(status->cur_log)++]=str[i];
|
|
}
|
|
if(status->cur_log >= LOG_LENTH){
|
|
status->cur_log = 0;
|
|
}
|
|
status->log[(status->cur_log)++]=str[i];
|
|
status->log[(status->cur_log)]='\0';
|
|
|
|
}
|
|
|
|
void step_vehicle(struct vehicle *v, int action){
|
|
//float action_x[NB_ACTION]={-3,0,3}; // [LEFT, CENTER, RIGHT]
|
|
float action_x[NB_ACTION]={-3,0,3}; // [LEFT, CENTER, RIGHT]
|
|
v->direction = v->direction + action_x[action % 3];
|
|
v->speed = ((float)1)/5;
|
|
move_vehicle(v);
|
|
read_sensor(v);
|
|
struct game_status *status = v->status;
|
|
status->q_state = v->sensor->x[LEFT]* 2500 +
|
|
v->sensor->x[CENTER]* 50 +
|
|
v->sensor->x[RIGHT] ;
|
|
status->reward = 0;
|
|
status->done =false;
|
|
struct blocks * path = v->path;
|
|
//printf(" center : %f vs %f direction: %f\n",v->sensor->x[CENTER], LIMIT_DISTANCE, v->direction);
|
|
if( v->sensor->x[CENTER]<= LIMIT_DISTANCE ){
|
|
status->reward = REWARD_STOP;
|
|
status->done = true;
|
|
}
|
|
else{
|
|
bool broken = false;
|
|
long prec, next;
|
|
char msg[48];
|
|
for(long i=0; i< path->nb_blocks; ++i){
|
|
//prec = (i-1)%(path->nb_blocks);
|
|
prec = (i + path->nb_blocks - 1 )%(path->nb_blocks);
|
|
next = (i + 1)%(path->nb_blocks);
|
|
//printf("i:%ld, prec:%ld, next:%ld: maker %d, prec marker %d\n",i,prec,next, path->marker[i], path->marker[prec]);
|
|
if(is_in_block_index(path, i, v->coord)){
|
|
if(path->marker[i] == false && path->marker[prec] == true){
|
|
path->marker[i]=true;
|
|
path->marker[prec]=false;
|
|
status->reward = REWARD_CONTINUE;
|
|
status->done = false;
|
|
sprintf(msg," %ld,",i);
|
|
add_string_log(status, msg);
|
|
}
|
|
if(path->marker[next] == true){
|
|
status->reward = REWARD_STOP;
|
|
status->done = true;
|
|
add_string_log(status, "| reverse |");
|
|
}
|
|
broken = true;
|
|
break;
|
|
}
|
|
}
|
|
if(broken == false){
|
|
if(status->cumulative_reward > THRESHOLD_REWARD){
|
|
status->reward = REWARD_CONTINUE;
|
|
status->done = true;
|
|
}
|
|
}
|
|
}
|
|
status->cumulative_reward += status->reward;
|
|
|
|
}
|
|
|
|
|
|
|
|
void reset(struct vehicle *v){
|
|
struct blocks * path = v->path;
|
|
long int i;
|
|
for(i=0; i<(path->nb_blocks -1); ++i)
|
|
path->marker[i] = false;
|
|
path->marker[i] = true;
|
|
v->status->cumulative_reward = 0;
|
|
sprintf(v->status->log,"\n");
|
|
v->status->cur_log = 0;
|
|
srand(time(NULL));
|
|
int random;
|
|
int diff;
|
|
diff = path->upper_bound_block[0]->x[0] - path->lower_bound_block[0]->x[0];
|
|
random = rand() % diff;
|
|
//v->coord->x[0] = path->lower_bound_block[0]->x[0] + random;
|
|
v->coord->x[0] = path->lower_bound_block[0]->x[0] + diff/2;
|
|
diff = path->upper_bound_block[0]->x[1] - path->lower_bound_block[0]->x[1];
|
|
random = rand() % diff;
|
|
//v->coord->x[1] = path->lower_bound_block[0]->x[1] + random;
|
|
v->coord->x[1] = path->lower_bound_block[0]->x[1] + diff/2;
|
|
random = rand() % 50;
|
|
//v->direction = random - 25;
|
|
v->direction = -90;
|
|
v->speed = 1;
|
|
}
|