-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathdrawHeadingArrow.m
145 lines (124 loc) · 3.51 KB
/
drawHeadingArrow.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
function [ha] = drawHeadingArrow(hAx, origin, psi, arrowSize, color, useQuiver)
% DRAWHEADINGARROW draws an annotation arrow at the heading psi
%
% If useQuiver is set, the plot does not have to be normalized.
%% ================ Dependencies ===============
% Find all dependencies
% Add lib and class folders
addpath('lib','class');
% =================== Check Arguments ========================
if nargin < 1
error('No input arguments given!');
elseif nargin > 6
error('Too many arguments given!');
end
if isempty(hAx)
error('hAx not given!');
end
if isempty(origin)
error('origin is empty!');
end
if isempty(psi)
error ('psi not given!');
end
if isempty(arrowSize)
error('arrowSize not given!');
end
if (isempty(color))
error('color not given!');
end
if nargin < 6
useQuiver = false;
end
headLength = 10; %arrowSize *2;
headWidth = 10; %arrowSize *2;
% ================== Draw Arrow =======================
ox = origin(1);
oy = origin(2);
theta = heading2angle(psi);
%
% if (psi < 0 || psi >= 2*pi)
% error('Unexpected psi value: %f', psi);
% elseif (psi < pi/2)
% px = ox + arrowSize*sin(psi);
% py = oy + arrowSize*sin(pi/2 - psi);
% elseif (psi < pi)
% psi = psi - pi/2;
% px = ox + arrowSize*sin(pi/2 - psi);
% py = oy - arrowSize*sin(psi);
% elseif (psi < 3*pi/2)
% psi = psi - pi;
% px = ox - arrowSize*sin(psi);
% py = oy - arrowSize*sin(pi/2 - psi);
% else
% psi = psi - 3*pi/2;
% px = ox - arrowSize*sin(pi/2 - psi);
% py = oy + arrowSize*sin(psi);
% end
%[of, pf] = dsxy2figxy(gca, [ox oy], [px py])
%oxf = of(1), oyf = of(2)
%pxf = pf(1), pyf = pf(2)
%[pxf, pyf] = dsxy2figxy(hAx, px, py);
%[oxf, oyf] = ds2nfu(hAx, ox, oy)
%[pxf, pyf] = ds2nfu(hAx, px, py)
% annotation('textarrow',ox,oy,'String','y = x ')
% ox, px, oy, py
% hold on;
% plot(ox, oy, '*g');
% hold off;
% [of pf] = axescoord2figurecoord([ox px], [oy py], hAx);
% oxf = of(1);
% oyf = of(2);
% pxf = pf(1);
% pyf = pf(2);
% annotation_pinned('arrow', [oxf pxf], [oyf pyf], 'Color', color,...
% 'HeadLength', headLength, 'HeadWidth', headWidth, 'axes', hAx);
if (~useQuiver)
%[dx, dy] = pol2cart(theta, arrowSize);
%px = ox + dx;
%py = ox + dy;
[px, py] = getArrowEndpoint([ox, oy], psi, arrowSize/10);
[oxf, oyf] = ds2nfu(hAx, ox, oy);
[pxf, pyf] = ds2nfu(hAx, px, py);
ha = annotation('arrow', [oxf pxf], [oyf pyf], 'Color', color,...
'HeadLength', headLength, 'HeadWidth', headWidth);
else
u = arrowSize*cos(theta);
v = arrowSize*sin(theta);
hold on;
ha = quiver(ox, oy, u, v, arrowSize, 'AutoScale', 'off', 'Color', color,...
'MaxHeadSize', 1e2);
hold off;
end
%set(qv,'MaxHeadSize',arrowSize/2);
%set(qv,'Color', color);
% Works for non-normalized
% FIXME add an argument to use this instead
%%vectarrow([ox oy], [px py]);
% annotation_pinned('arrow', [oxf pxf], [oyf pyf], 'Color', color,...
% 'HeadLength', headLength, 'HeadWidth', headWidth, 'axes', hAx);
end % function drawHeadingArrow()
function [px, py] = getArrowEndpoint(origin, psi, s)
ox = origin(1);
oy = origin(2);
px = ox;
py = oy;
if (psi < 0 || psi >= 2*pi)
error('Unexpected psi value: %f', psi);
elseif (psi < pi/2)
px = ox + s*sin(psi);
py = oy + s*sin(pi/2 - psi);
elseif (psi < pi)
psi = psi - pi/2;
px = ox + s*sin(pi/2 - psi);
py = oy - s*sin(psi);
elseif (psi < 3*pi/2)
psi = psi - pi;
px = ox - s*sin(psi);
py = oy - s*sin(pi/2 - psi);
else
psi = psi - 3*pi/2;
px = ox - s*sin(pi/2 - psi);
py = oy + s*sin(psi);
end
end % function