-
Notifications
You must be signed in to change notification settings - Fork 0
/
planning_util.c
84 lines (68 loc) · 2.42 KB
/
planning_util.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
/**
* To help with the intelligence and path planning components of Mapkin.
*
* Contains functions to decide where to go next, and to command the robot
* to move.
*/
#include "mapkin.h"
void setGoal(int* grid, pose2D* cur_pos, gridPoint* goal_pos)
{
float theta, m, m2, x, y, a;
int i, j;
int curX = (int)(-(cur_pos->x/CELL_WIDTH) + (GRIDSIZE/2) - .5);
int curY = (int)(-(cur_pos->y/CELL_WIDTH) + (GRIDSIZE/2) - .5);
findNearest(grid, EMPTY, &curX, &curY);
goal_pos->x = curX;
goal_pos->y = curY;
/*for(theta=0; theta<2*PI; theta += .01)
{
// Calculate the line in point-slope form
if(fabs(cos(theta)) < 0.0005)
m = 10000000;
else
m = sin(theta)/cos(theta);
if(fabs(sin(theta)) < 0.0005)
m2 = 10000000;
else
m2 = cos(theta)/sin(theta);
// determine quadrant
int xSign = 1;
int ySign = 1;
if(theta < PI && theta >= PI / 2)
xSign = -1;
else if(theta < 3*PI / 2 && theta >= PI)
xSign = ySign = -1;
else if(theta < 2*PI && theta >= 3*PI/2)
ySign = -1;
for(a=0; ; a+=.3)
{
int gridX, gridY;
// Use x if the line is more horizontal
if(abs(m) <= 1)
{
x = xSign * a + goal_pos->x;
y = m*(x - goal_pos->x) + goal_pos->y;
}
else
{
y = ySign * a + goal_pos->y;
x = m2*(y - goal_pos->y) + goal_pos->x;
}
gridX = (int)x; //(-(x/CELL_WIDTH) + (GRIDSIZE/2) - .5);
gridY = (int)y; //(-(y/CELL_WIDTH) + (GRIDSIZE/2) - .5);
if(gridX < 0 || gridX >= GRIDSIZE || gridY < 0 || gridY >= GRIDSIZE)
break;
int pointStatus = grid[XYtoIDX(gridX,gridY)];
if(pointStatus == SEEN || pointStatus == VISITED || pointStatus == EMPTY
|| pointStatus == BLOCKED_SEEN || pointStatus == BLOCKED_EMPTY)
//if(grid[gridX][gridY] == SEEN || grid[gridX][gridY] == VISITED
// || grid[gridX][gridY] == EMPTY
// || grid[gridX][gridY] == BLOCKED_SEEN
// || grid[gridX][gridY] == BLOCKED_EMPTY)
{
// previously set approach points here
}
}
}*/
//printf("Set goal to (%d,%d)\n", goal_pos->x, goal_pos->y);
}