-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdubins_types.m
252 lines (252 loc) · 17.9 KB
/
dubins_types.m
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
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
function total_discrete_trajectory = dubins_types(start,finish,index)
%%
all_dubins_types = ["LSL","RSR","LSR","RSL"];
dubins_type = all_dubins_types(index);
if dubins_type == "LSL"
%---------------------------------------------------------------------LSL-----------------------------------------------------------------------------
clear total_discrete_trajectory;
plot(start.x,start.y,'*b');
hold on;
plot(finish.x,finish.y,'g*');
axis([min(start.x,finish.x) - 3 * 1 / min(start.curvature,finish.curvature),max(start.x,finish.x) + 3 * 1 / min(start.curvature,finish.curvature),min(start.y,finish.y) - 3 * 1 / min(start.curvature,finish.curvature),max(start.y,finish.y) + 3 * 1 / min(start.curvature,finish.curvature)]);
drawArrow([start.x,start.y],[start.x + cos(start.direction_speed) * 2,start.y + sin(start.direction_speed) * 2],'b','b',3,2);
hold on;
drawArrow([finish.x,finish.y],[finish.x + cos(finish.direction_speed) * 2,finish.y + sin(finish.direction_speed) * 2],'g','g',3,2);
title(['LSL ',num2str(1 / start.curvature)]);
%%
%get primary circles
start.primary_centre_x = start.x - 1 / start.curvature * cos(start.direction_speed - pi / 2);
start.primary_centre_y = start.y - 1 / start.curvature * sin(start.direction_speed - pi / 2);
rectangle('Position',[start.primary_centre_x - 1 / start.curvature,start.primary_centre_y - 1 / start.curvature,2 / start.curvature,2 / start.curvature],'Curvature',[1,1])
finish.primary_centre_x = finish.x - 1 / finish.curvature * cos(finish.direction_speed - pi / 2);
finish.primary_centre_y = finish.y - 1 / finish.curvature * sin(finish.direction_speed - pi / 2);
rectangle('Position',[finish.primary_centre_x - 1 / finish.curvature,finish.primary_centre_y - 1 / finish.curvature,2 / finish.curvature,2 / finish.curvature],'Curvature',[1,1])
hold on;
%%
%get exit point and enter point
alpha = asin((1 / finish.curvature - 1 / start.curvature) / sqrt((start.primary_centre_x - finish.primary_centre_x)^2 + (start.primary_centre_y - finish.primary_centre_y)^2));
centerline_slope = atan2((finish.primary_centre_y - start.primary_centre_y) , (finish.primary_centre_x - start.primary_centre_x));
start.turn_angle = -alpha + centerline_slope + 3 * pi / 2;
finish.turn_angle = -alpha + centerline_slope + 3 * pi / 2;
start.exit_point_x = start.primary_centre_x + 1 / start.curvature * cos(start.turn_angle);
start.exit_point_y = start.primary_centre_y + 1 / start.curvature * sin(start.turn_angle);
finish.enter_point_x = finish.primary_centre_x + 1 / finish.curvature * cos(finish.turn_angle);
finish.enter_point_y = finish.primary_centre_y + 1 / finish.curvature * sin(finish.turn_angle);
plot(start.exit_point_x,start.exit_point_y,'ro');
hold on;
plot(finish.enter_point_x,finish.enter_point_y,'go');
hold on;
start.rotate_angle = mod(-start.direction_speed + pi / 2 + start.turn_angle,2 * pi);
finish.rotate_angle = mod(finish.direction_speed - pi / 2 - finish.turn_angle,2 * pi);
%%
%plot trajectory
[start_x,start_y] = drawTrajectory([start.x,start.y],[start.primary_centre_x,start.primary_centre_y],1 / start.curvature,start.rotate_angle,0);
%plot(start_x,start_y,'r','LineWidth',2);
%hold on;
[finish_x,finish_y] = drawTrajectory([finish.enter_point_x,finish.enter_point_y],[finish.primary_centre_x,finish.primary_centre_y],1 / finish.curvature,finish.rotate_angle,0);
%plot(finish_x,finish_y,'r','LineWidth',2);
%hold on;
%line([start.exit_point_x,finish.enter_point_x],[start.exit_point_y,finish.enter_point_y],'Color','r','LineWidth',2);
%hold on;
%%
%get discrete trajectory
delta_line =pi / 100;
theta_line = atan2((finish.enter_point_y - start.exit_point_y),(finish.enter_point_x - start.exit_point_x));
line_discrete_x = start.exit_point_x:delta_line * cos(theta_line):finish.enter_point_x;
line_discrete_y = start.exit_point_y:delta_line * sin(theta_line):finish.enter_point_y;
total_discrete_trajectory(1,:) = [start_x,line_discrete_x,finish_x];
total_discrete_trajectory(2,:) = [start_y,line_discrete_y,finish_y];
plot(total_discrete_trajectory(1,:),total_discrete_trajectory(2,:),'r','LineWidth',2);
hold on;
%%
%get the length of trajectory
dubins_length.LSL = start.rotate_angle * 1 / start.curvature + sqrt((finish.enter_point_x - start.exit_point_x)^2 + (finish.enter_point_y - start.exit_point_y)^2) + finish.rotate_angle * 1 / finish.curvature;
%text(25,40,num2str(dubins_length.LSL),'FontSize',15,'FontWeight','Bold','Color','r');
elseif dubins_type == "RSR"
%%
%----------------------------------------------------------------------RSR----------------------------------------------------------------------------
clear total_discrete_trajectory;
plot(start.x,start.y,'*b');
hold on;
plot(finish.x,finish.y,'g*');
axis([min(start.x,finish.x) - 3 * 1 / min(start.curvature,finish.curvature),max(start.x,finish.x) + 3 * 1 / min(start.curvature,finish.curvature),min(start.y,finish.y) - 3 * 1 / min(start.curvature,finish.curvature),max(start.y,finish.y) + 3 * 1 / min(start.curvature,finish.curvature)]);
drawArrow([start.x,start.y],[start.x + cos(start.direction_speed) * 2,start.y + sin(start.direction_speed) * 2],'b','b',3,2);
hold on;
drawArrow([finish.x,finish.y],[finish.x + cos(finish.direction_speed) * 2,finish.y + sin(finish.direction_speed) * 2],'g','g',3,2);
title(['RSR ',num2str(1 / start.curvature)]);
%%
%get primary circles
start.primary_centre_x = start.x - 1 / start.curvature * cos(start.direction_speed + pi / 2);
start.primary_centre_y = start.y - 1 / start.curvature * sin(start.direction_speed + pi / 2);
rectangle('Position',[start.primary_centre_x - 1 / start.curvature,start.primary_centre_y - 1 / start.curvature,2 / start.curvature,2 / start.curvature],'Curvature',[1,1])
finish.primary_centre_x = finish.x - 1 / finish.curvature * cos(finish.direction_speed + pi / 2);
finish.primary_centre_y = finish.y - 1 / finish.curvature * sin(finish.direction_speed + pi / 2);
rectangle('Position',[finish.primary_centre_x - 1 / finish.curvature,finish.primary_centre_y - 1 / finish.curvature,2 / finish.curvature,2 / finish.curvature],'Curvature',[1,1])
hold on;
%%
%get exit point and enter point
alpha = asin((1 / finish.curvature - 1 / start.curvature) / sqrt((start.primary_centre_x - finish.primary_centre_x)^2 + (start.primary_centre_y - finish.primary_centre_y)^2));
centerline_slope = atan2((finish.primary_centre_y - start.primary_centre_y) , (finish.primary_centre_x - start.primary_centre_x));
%start.turn_angle = -alpha + centerline_slope + 3 * pi / 2;
%finish.turn_angle = -alpha + centerline_slope + 3 * pi / 2;
start.turn_angle = alpha + centerline_slope + pi /2;
finish.turn_angle = alpha + centerline_slope + pi /2;
start.exit_point_x = start.primary_centre_x + 1 / start.curvature * cos(start.turn_angle);
start.exit_point_y = start.primary_centre_y + 1 / start.curvature * sin(start.turn_angle);
finish.enter_point_x = finish.primary_centre_x + 1 / finish.curvature * cos(finish.turn_angle);
finish.enter_point_y = finish.primary_centre_y + 1 / finish.curvature * sin(finish.turn_angle);
plot(start.exit_point_x,start.exit_point_y,'ro');
hold on;
plot(finish.enter_point_x,finish.enter_point_y,'go');
hold on;
start.rotate_angle = mod(start.direction_speed + pi / 2 - start.turn_angle,2 * pi);
finish.rotate_angle = mod(-finish.direction_speed - pi / 2 + finish.turn_angle,2 * pi);
%%
%plot trajectory
[start_x,start_y] = drawTrajectory([start.x,start.y],[start.primary_centre_x,start.primary_centre_y],1 / start.curvature,start.rotate_angle,1);
plot(start_x,start_y,'r','LineWidth',2);
%hold on;
[finish_x,finish_y] = drawTrajectory([finish.enter_point_x,finish.enter_point_y],[finish.primary_centre_x,finish.primary_centre_y],1 / finish.curvature,finish.rotate_angle,1);
%plot(finish_x,finish_y,'r','LineWidth',2);
%hold on;
%line([start.exit_point_x,finish.enter_point_x],[start.exit_point_y,finish.enter_point_y],'Color','r','LineWidth',2);
%hold on;
%%
%get discrete trajectory
delta_line =pi / 100;
theta_line = atan2((finish.enter_point_y - start.exit_point_y),(finish.enter_point_x - start.exit_point_x));
line_discrete_x = start.exit_point_x:delta_line * cos(theta_line):finish.enter_point_x;
line_discrete_y = start.exit_point_y:delta_line * sin(theta_line):finish.enter_point_y;
total_discrete_trajectory(1,:) = [start_x,line_discrete_x,finish_x];
total_discrete_trajectory(2,:) = [start_y,line_discrete_y,finish_y];
plot(total_discrete_trajectory(1,:),total_discrete_trajectory(2,:),'r','LineWidth',2);
hold on;
%%
%get the length of trajectory
dubins_length.RSR = start.rotate_angle * 1 / start.curvature + sqrt((finish.enter_point_x - start.exit_point_x)^2 + (finish.enter_point_y - start.exit_point_y)^2) + finish.rotate_angle * 1 / finish.curvature;
%text(25,40,num2str(dubins_length.RSR),'FontSize',15,'FontWeight','Bold','Color','r');
elseif dubins_type == "LSR"
%%
%----------------------------------------------------------------------LSR----------------------------------------------------------------------------
clear total_discrete_trajectory;
plot(start.x,start.y,'*b');
hold on;
plot(finish.x,finish.y,'g*');
axis([min(start.x,finish.x) - 3 * 1 / min(start.curvature,finish.curvature),max(start.x,finish.x) + 3 * 1 / min(start.curvature,finish.curvature),min(start.y,finish.y) - 3 * 1 / min(start.curvature,finish.curvature),max(start.y,finish.y) + 3 * 1 / min(start.curvature,finish.curvature)]);
drawArrow([start.x,start.y],[start.x + cos(start.direction_speed) * 2,start.y + sin(start.direction_speed) * 2],'b','b',3,2);
hold on;
drawArrow([finish.x,finish.y],[finish.x + cos(finish.direction_speed) * 2,finish.y + sin(finish.direction_speed) * 2],'g','g',3,2);
title(['LSR ',num2str(1 / start.curvature)]);
%%
%get primary circles
start.primary_centre_x = start.x - 1 / start.curvature * cos(start.direction_speed - pi / 2);
start.primary_centre_y = start.y - 1 / start.curvature * sin(start.direction_speed - pi / 2);
rectangle('Position',[start.primary_centre_x - 1 / start.curvature,start.primary_centre_y - 1 / start.curvature,2 / start.curvature,2 / start.curvature],'Curvature',[1,1])
finish.primary_centre_x = finish.x - 1 / finish.curvature * cos(finish.direction_speed + pi / 2);
finish.primary_centre_y = finish.y - 1 / finish.curvature * sin(finish.direction_speed + pi / 2);
rectangle('Position',[finish.primary_centre_x - 1 / finish.curvature,finish.primary_centre_y - 1 / finish.curvature,2 / finish.curvature,2 / finish.curvature],'Curvature',[1,1])
hold on;
%%
%get exit point and enter point
alpha = asin((1 / finish.curvature + 1 / start.curvature) / sqrt((start.primary_centre_x - finish.primary_centre_x)^2 + (start.primary_centre_y - finish.primary_centre_y)^2));
centerline_slope = atan2((finish.primary_centre_y - start.primary_centre_y) , (finish.primary_centre_x - start.primary_centre_x));
start.turn_angle = alpha + centerline_slope - pi / 2;
%finish.turn_angle = -alpha + centerline_slope + 3 * pi / 2;
%start.turn_angle = alpha + centerline_slope + pi /2;
finish.turn_angle = alpha + centerline_slope + pi / 2;
start.exit_point_x = start.primary_centre_x + 1 / start.curvature * cos(start.turn_angle);
start.exit_point_y = start.primary_centre_y + 1 / start.curvature * sin(start.turn_angle);
finish.enter_point_x = finish.primary_centre_x + 1 / finish.curvature * cos(finish.turn_angle);
finish.enter_point_y = finish.primary_centre_y + 1 / finish.curvature * sin(finish.turn_angle);
plot(start.exit_point_x,start.exit_point_y,'ro');
hold on;
plot(finish.enter_point_x,finish.enter_point_y,'go');
hold on;
start.rotate_angle = mod(-start.direction_speed + pi / 2 + start.turn_angle,2 * pi);
finish.rotate_angle = mod(-finish.direction_speed - pi / 2 + finish.turn_angle,2 * pi);
%%
%plot trajectory
[start_x,start_y] = drawTrajectory([start.x,start.y],[start.primary_centre_x,start.primary_centre_y],1 / start.curvature,start.rotate_angle,0);
%plot(start_x,start_y,'r','LineWidth',2);
%hold on;
[finish_x,finish_y] = drawTrajectory([finish.enter_point_x,finish.enter_point_y],[finish.primary_centre_x,finish.primary_centre_y],1 / finish.curvature,finish.rotate_angle,1);
%plot(finish_x,finish_y,'r','LineWidth',2);
%hold on;
%line([start.exit_point_x,finish.enter_point_x],[start.exit_point_y,finish.enter_point_y],'Color','r','LineWidth',2);
%hold on;
%%
%get discrete trajectory
delta_line =pi / 100;
theta_line = atan2((finish.enter_point_y - start.exit_point_y),(finish.enter_point_x - start.exit_point_x));
line_discrete_x = start.exit_point_x:delta_line * cos(theta_line):finish.enter_point_x;
line_discrete_y = start.exit_point_y:delta_line * sin(theta_line):finish.enter_point_y;
total_discrete_trajectory(1,:) = [start_x,line_discrete_x,finish_x];
total_discrete_trajectory(2,:) = [start_y,line_discrete_y,finish_y];
plot(total_discrete_trajectory(1,:),total_discrete_trajectory(2,:),'r','LineWidth',2);
hold on;
%%
%get the length of trajectory
dubins_length.LSR = start.rotate_angle * 1 / start.curvature + sqrt((finish.enter_point_x - start.exit_point_x)^2 + (finish.enter_point_y - start.exit_point_y)^2) + finish.rotate_angle * 1 / finish.curvature;
%text(25,40,num2str(dubins_length.LSR),'FontSize',15,'FontWeight','Bold','Color','r');
else
%%
%----------------------------------------------------------------------RSL----------------------------------------------------------------------------
clear total_discrete_trajectory;
plot(start.x,start.y,'*b');
hold on;
plot(finish.x,finish.y,'g*');
axis([min(start.x,finish.x) - 3 * 1 / min(start.curvature,finish.curvature),max(start.x,finish.x) + 3 * 1 / min(start.curvature,finish.curvature),min(start.y,finish.y) - 3 * 1 / min(start.curvature,finish.curvature),max(start.y,finish.y) + 3 * 1 / min(start.curvature,finish.curvature)]);
drawArrow([start.x,start.y],[start.x + cos(start.direction_speed) * 2,start.y + sin(start.direction_speed) * 2],'b','b',3,2);
hold on;
drawArrow([finish.x,finish.y],[finish.x + cos(finish.direction_speed) * 2,finish.y + sin(finish.direction_speed) * 2],'g','g',3,2);
title(['RSL ',num2str(1 / start.curvature)]);
%%
%get primary circles
start.primary_centre_x = start.x - 1 / start.curvature * cos(start.direction_speed + pi / 2);
start.primary_centre_y = start.y - 1 / start.curvature * sin(start.direction_speed + pi / 2);
rectangle('Position',[start.primary_centre_x - 1 / start.curvature,start.primary_centre_y - 1 / start.curvature,2 / start.curvature,2 / start.curvature],'Curvature',[1,1])
finish.primary_centre_x = finish.x - 1 / finish.curvature * cos(finish.direction_speed - pi / 2);
finish.primary_centre_y = finish.y - 1 / finish.curvature * sin(finish.direction_speed - pi / 2);
rectangle('Position',[finish.primary_centre_x - 1 / finish.curvature,finish.primary_centre_y - 1 / finish.curvature,2 / finish.curvature,2 / finish.curvature],'Curvature',[1,1])
hold on;
%%
%get exit point and enter point
alpha = asin((1 / finish.curvature + 1 / start.curvature) / sqrt((start.primary_centre_x - finish.primary_centre_x)^2 + (start.primary_centre_y - finish.primary_centre_y)^2));
centerline_slope = atan2((finish.primary_centre_y - start.primary_centre_y) , (finish.primary_centre_x - start.primary_centre_x));
finish.turn_angle = -alpha + centerline_slope + 3 * pi / 2;
start.turn_angle = -alpha + centerline_slope + pi /2;
start.exit_point_x = start.primary_centre_x + 1 / start.curvature * cos(start.turn_angle);
start.exit_point_y = start.primary_centre_y + 1 / start.curvature * sin(start.turn_angle);
finish.enter_point_x = finish.primary_centre_x + 1 / finish.curvature * cos(finish.turn_angle);
finish.enter_point_y = finish.primary_centre_y + 1 / finish.curvature * sin(finish.turn_angle);
plot(start.exit_point_x,start.exit_point_y,'ro');
hold on;
plot(finish.enter_point_x,finish.enter_point_y,'go');
hold on;
start.rotate_angle = mod(start.direction_speed + pi / 2 - start.turn_angle,2 * pi);
finish.rotate_angle = mod(finish.direction_speed - pi / 2 - finish.turn_angle,2 * pi);
%%
%plot trajectory
[start_x,start_y] = drawTrajectory([start.x,start.y],[start.primary_centre_x,start.primary_centre_y],1 / start.curvature,start.rotate_angle,1);
%plot(start_x,start_y,'r','LineWidth',2);
%hold on;
[finish_x,finish_y] = drawTrajectory([finish.enter_point_x,finish.enter_point_y],[finish.primary_centre_x,finish.primary_centre_y],1 / finish.curvature,finish.rotate_angle,0);
%plot(finish_x,finish_y,'r','LineWidth',2);
%hold on;
%line([start.exit_point_x,finish.enter_point_x],[start.exit_point_y,finish.enter_point_y],'Color','r','LineWidth',2);
%hold on;
%%
%get discrete trajectory
delta_line =pi / 100;
theta_line = atan2((finish.enter_point_y - start.exit_point_y),(finish.enter_point_x - start.exit_point_x));
line_discrete_x = start.exit_point_x:delta_line * cos(theta_line):finish.enter_point_x;
line_discrete_y = start.exit_point_y:delta_line * sin(theta_line):finish.enter_point_y;
total_discrete_trajectory(1,:) = [start_x,line_discrete_x,finish_x];
total_discrete_trajectory(2,:) = [start_y,line_discrete_y,finish_y];
plot(total_discrete_trajectory(1,:),total_discrete_trajectory(2,:),'r','LineWidth',2);
hold on;
%%
%get the length of trajectory
dubins_length.RSL = start.rotate_angle * 1 / start.curvature + sqrt((finish.enter_point_x - start.exit_point_x)^2 + (finish.enter_point_y - start.exit_point_y)^2) + finish.rotate_angle * 1 / finish.curvature;
%text(25,40,num2str(dubins_length.RSL),'FontSize',15,'FontWeight','Bold','Color','r');
end