void choose_direction_for_ghost(char * map, struct pacghost * pacman, struct pacghost * ghost, int difficulty, int is_pacman_powered_up){
if (!is_pacman_powered_up){
if (difficulty == 0){
move_randomly(ghost);
} else if (difficulty == 1){
int random = random_in_range(0,10);
if (random < 5)
move_randomly(ghost);
else
chase_after_pacman(pacman, ghost);
} else if (difficulty == 2){
int random = random_in_range(0,10);
if (random < 2)
move_randomly(ghost);
else
chase_after_pacman(pacman, ghost);
} else {
chase_after_pacman(pacman, ghost);
}
/* The ghost move to the point nearer to him than pacman when pacman powered */
} else {
/* As the point search in order. The ghost tend to stuck when came to top left */
for (int rows = 0; rows < height; rows++) {
for (int cols = 0; cols < width; cols++){
if (isValidMoveCell(rows, cols)){
if (distance_between_two_points(pacman->xLocation,pacman->yLocation,rows,cols) > distance_between_two_points(ghost->xLocation,ghost->yLocation,rows,cols)){
ghost->direction = direction_from_node_to_node(ghost->xLocation,ghost->yLocation,rows,cols);
return;
}
}
}
}
}
}
void distance_by_vector_projection_angle_between_robot_and_direction (float final_x, float final_y, float robot_x, float robot_y, float robot_angle, float *distance, float *angle)
{
float scalar_product=0.0;
float angle_sign =0.0;
scalar_product = scalar_product_between_robot_and_direction(final_x, final_y, robot_x, robot_y, robot_angle);
*distance = distance_between_two_points( final_x, final_y, robot_x, robot_y);
// Calcul de la valeur absolue de l'angle à parcourir avec le produit scalaire
*angle = acos( scalar_product / *distance );
// Calcul du sinus de l'angle à parcourir pour avoir le sens de rotation. Avec la produit vectoriel
angle_sign = vector_product_between_robot_and_direction(final_x, final_y, robot_x, robot_y, robot_angle); // ATTENTION : on avait divisé par error_distance .. normalement ca ne sert à rien mais au cas ou ... / error_distance );
// Détermination si l'on doit reculer plutôt qu'avancer.
if (*angle >= M_PI/2) // quart arrière gauche
{
*angle = *angle - M_PI; // on replace l'angle devant le robot
*distance = (*distance) * (-1); // on recule
}
if ( angle_sign < 0.0 )
{
*angle = (*angle) * (-1); // Application du signe.
}
}
// Distance + Angle in theta-alpha control
/// Improvements : parameter for the final approach distance
unsigned char mode_3_control_motion(StructPos *psetpoint, StructPos *pcurrent, float *raw_command_right, float *raw_command_left)
{
float error_angle=0.0;
float error_distance=0.0;
float error_filtered_angle=0.0;
float error_filtered_distance=0.0;
float speed_ratio_distance = DEFAULT_SPEED_DISTANCE;
unsigned char end_movement_flag=0;
// Compute error
// Compute distance : ABS value for vectorial considerations
error_distance = distance_between_two_points( psetpoint->x, psetpoint->y, pcurrent->x, pcurrent->y);
if(error_distance<= DISTANCE_ALPHA_ONLY) // final distance reached
{
error_angle=angle_between_two_points(psetpoint->angle, pcurrent->angle);
if(error_angle<=ANGLE_APPROACH_PRECISION) end_movement_flag=1;
error_distance = scalar_product_between_robot_and_direction(psetpoint->x, psetpoint->y, pcurrent->x, pcurrent->y, pcurrent->angle);
}
else
{
distance_by_vector_projection_angle_between_robot_and_direction(psetpoint->x, psetpoint->y, pcurrent->x, pcurrent->y, pcurrent->angle, &error_distance, &error_angle);
}
// QUADRAMP filter on errors
speed_ratio_distance = QUADRAMP_Compute(&distance_quadramp_data, error_distance);
// PID filter on errors
error_filtered_angle = PID_Computation(&angle_pid_data, error_angle);
error_filtered_distance = PID_Computation(&distance_pid_data, error_distance);
// Re-scale errors to fit on expected scale
error_filtered_distance = error_rescale (error_filtered_distance, (1.0 - ANGLE_VS_DISTANCE_RATIO), speed_ratio_distance);
error_filtered_angle = error_rescale (error_filtered_angle, ANGLE_VS_DISTANCE_RATIO, SPEED_ANGLE);
// Command merge
*raw_command_right = error_filtered_distance + error_filtered_angle;
*raw_command_left = error_filtered_distance - error_filtered_angle;
return end_movement_flag;
}
int direction_from_node_to_node(int xStart, int yStart, int xDestination, int yDestination){
if (isValidMoveCell(xStart, yStart) && isValidMoveCell(xDestination,yDestination)){
struct list * open_list = list_create();
struct list * close_list = list_create();
struct node * new_node = node_create();
new_node->x = xStart;
new_node->y = yStart;
new_node->g = 0;
new_node->h = distance_between_two_points(xStart,yStart,xDestination,yDestination);
new_node->f = new_node->h;
open_list->root = new_node;
while (open_list->root != NULL){
struct node * q = node_with_least_f(open_list);
pop_a_node_off_list(open_list,q);
int availableWay = ghost_map[q->x * width + q->y] - '0';
int up_added = 0;
int right_added = 0;
int down_added = 0;
struct node * sucessor[availableWay];
for (int i = 0; i < availableWay; i++){
sucessor[i] = node_create();
sucessor[i]->parent = q;
if (isValidMoveCell(q->x-1,q->y) && (up_added == 0)) {
sucessor[i]->x = q->x-1;
sucessor[i]->y = q->y;
up_added = 1;
} else if (isValidMoveCell(q->x, q->y+1) && (right_added == 0)){
sucessor[i]->x = q->x;
sucessor[i]->y = q->y+1;
right_added = 1;
} else if (isValidMoveCell(q->x+1, q->y) && (down_added == 0)){
sucessor[i]->x = q->x+1;
sucessor[i]->y = q->y;
down_added = 1;
} else if (isValidMoveCell(q->x, q->y-1)){
sucessor[i]->x = q->x;
sucessor[i]->y = q->y-1;
}
if (sucessor[i]->x == xDestination && sucessor[i]->y == yDestination) {
struct node * start_node = starting_node(sucessor[i]);
if (start_node->x < xStart){
free_node_and_list(sucessor[i],q,open_list,close_list);
return 0;
} else if (start_node->x > xStart){
free_node_and_list(sucessor[i],q,open_list,close_list);
return 2;
} else if (start_node->y > yStart){
free_node_and_list(sucessor[i],q,open_list,close_list);
return 1;
} else {
free_node_and_list(sucessor[i],q,open_list,close_list);
return 3;
}
} else {
sucessor[i]->g = q->g + distance_between_two_points(q->x, q->y, sucessor[i]->x, sucessor[i]->y);
sucessor[i]->h = distance_between_two_points(sucessor[i]->x,sucessor[i]->y, xDestination, yDestination);
sucessor[i]->f = sucessor[i]->g + sucessor[i]->h;
if ((ghost_map[sucessor[i]->x * width + sucessor[i]->y] - '0') == 1)
add_a_node_to_list(close_list,sucessor[i]);
else if (better_node_exist(open_list,sucessor[i]))
free(sucessor[i]);
else if (better_node_exist(close_list,sucessor[i]))
free(sucessor[i]);
else
add_a_node_to_list(open_list,sucessor[i]);
}
}
add_a_node_to_list(close_list, q);
}
}
return 4;
}
