125 lines
2.9 KiB
C
125 lines
2.9 KiB
C
//
|
|
// Created by junwei on 31/10/23.
|
|
//
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <stdlib.h>
|
|
|
|
#ifndef TEST_PROJECT_MAP_H
|
|
#define TEST_PROJECT_MAP_H
|
|
|
|
// Function to create and initialize a grid
|
|
void
|
|
map_init(int rows, int cols, grid_t *p_grid, int initial_x, int initial_y)
|
|
{
|
|
p_grid->rows = rows;
|
|
p_grid->cols = cols;
|
|
p_grid->initial_x = initial_x;
|
|
p_grid->initial_y = initial_y;
|
|
|
|
// Allocate memory for the grid
|
|
p_grid->data = (bool **)malloc(rows * sizeof(bool *));
|
|
for (int i = 0; i < rows; i++)
|
|
{
|
|
p_grid->data[i] = (bool *)malloc(cols * sizeof(bool));
|
|
for (int j = 0; j < cols; j++)
|
|
{
|
|
p_grid->data[i][j] = false; // Initialize to 'false' (unvisited)
|
|
}
|
|
}
|
|
}
|
|
|
|
// Function to mark a cell as visited
|
|
void
|
|
mark_cell(grid_t *grid, int row, int col)
|
|
{
|
|
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
|
|
{
|
|
grid->data[row][col] = true;
|
|
}
|
|
}
|
|
|
|
// Function to check if a cell has been visited
|
|
bool
|
|
is_cell_visited(grid_t *grid, int row, int col)
|
|
{
|
|
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
|
|
{
|
|
return grid->data[row][col];
|
|
}
|
|
return false; // Consider out-of-bounds as unvisited
|
|
}
|
|
|
|
// Function to destroy the grid and free memory
|
|
void
|
|
destroy_grid(grid_t *grid)
|
|
{
|
|
for (int i = 0; i < grid->rows; i++)
|
|
{
|
|
free(grid->data[i]);
|
|
}
|
|
free(grid->data);
|
|
free(grid);
|
|
}
|
|
|
|
// Function to update the map based on the car's current orientation and
|
|
// position
|
|
void
|
|
update_map(grid_t *car_path_grid, uint32_t direction, float distance)
|
|
{
|
|
int cur_x = car_path_grid->initial_x;
|
|
int cur_y = car_path_grid->initial_y;
|
|
|
|
// Define offsets for different forward and turn directions
|
|
int offset_x = 0;
|
|
int offset_y = 0;
|
|
|
|
// Update the current position based on the forward direction
|
|
switch (direction)
|
|
{
|
|
case DIRECTION_LEFT:
|
|
offset_x = -1;
|
|
break;
|
|
case DIRECTION_RIGHT:
|
|
offset_x = 1;
|
|
break;
|
|
case DIRECTION_FORWARD:
|
|
offset_y = 1;
|
|
break;
|
|
case DIRECTION_BACKWARD:
|
|
offset_y = -1;
|
|
break;
|
|
}
|
|
|
|
cur_x += offset_x;
|
|
cur_y += offset_y;
|
|
|
|
// Mark the current and next position as visited
|
|
mark_cell(car_path_grid, cur_x, cur_y);
|
|
car_path_grid->initial_x = cur_x;
|
|
car_path_grid->initial_y = cur_y;
|
|
car_path_grid->distance_array[car_path_grid->distance_array_size]
|
|
= distance;
|
|
car_path_grid->distance_array_size++;
|
|
|
|
|
|
}
|
|
|
|
// Function to print the map
|
|
void
|
|
print_map(grid_t *car_path_grid)
|
|
{
|
|
// Invert the map, 0,0 is at the Middle
|
|
// Print 1 for visited cells and 0 for unvisited cells
|
|
for (int i = car_path_grid->rows - 1; i >= 0; i--)
|
|
{
|
|
for (int j = 0; j < car_path_grid->cols; j++)
|
|
{
|
|
(car_path_grid->data[j][i]) ? printf("1 ") : printf("0 ");
|
|
}
|
|
printf("\n");
|
|
}
|
|
}
|
|
#endif // TEST_PROJECT_MAP_H
|