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
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409 | with Ada.Numerics;
with Ada.Numerics.Elementary_Functions;
with Agpl.Cv;
with Agpl.If_Function;
with Agpl.Interfaces.C.Types;
with Agpl.Strings;
-- with Agpl.Trace; use Agpl.Trace;
with Agpl.Transf2D;
package body Agpl.Drawing.Figures is
package Transform is new Transf2D (Float);
function "+" (P : in Cv.Point2D) return Transform.Pose;
pragma Inline ("+");
function "+" (P : in Transform.Pose) return Cv.Point2D;
pragma Inline ("+");
function "+" (P : in Cv.Point2D) return Transform.Pose is
begin
return (P (P'First), P (P'First + 1), 0.0, 1.0);
end "+";
function "+" (P : in Transform.Pose) return Cv.Point2D is
use Transform;
begin
return (P (X), P (Y), 1.0);
end "+";
-----------------
-- Null_Figure --
-----------------
type Null_Type is new Drawable with null record;
overriding
procedure Draw (Fig : Null_Type; D : in out Drawer'Class) is null;
function Null_Figure return Drawable'Class is
function Null_Figure_X return Null_Type is
begin
return N : Null_Type do
pragma Unreferenced (N);
null;
end return;
end Null_Figure_X;
begin
return Null_Figure_X; -- To avoid bug in GPL 2008
end Null_Figure;
----------
-- Text --
----------
type Text_Type (Length : Natural) is new Drawable with record
X, Y : Float;
Text : String (1 .. Length);
end record;
overriding
procedure Draw (Fig : Text_Type; D : in out Drawer'Class) is
begin
D.Write (Fig.X, Fig.Y, Fig.Text);
end Draw;
function Text (X, Y : Float; S : String) return Drawable'Class is
function Text_Internal return Text_Type is
begin
return Fig : Text_Type (S'Length) do
Fig := (S'Length, X, Y, S);
end return;
end Text_Internal;
begin
return Text_Internal;
end Text;
----------
-- Draw --
----------
procedure Draw (This : Vprogress; Into : in out Drawer'Class) is
use Agpl.Strings;
function Iif is new If_Function (String);
Engraving : constant := 0.08;
begin
Into.Set_Color ((0, 0, 0), 255);
Draw_Rectangle (Into, This.X, This.Y,
This.X + This.Width, This.Y + This.Height);
if This.Text_Above then
Into.Write
(This.X, This.Y + This.Height + 0.8,
To_String (This.Val, 1) &
Iif (This.Show_Pct,
" (" & To_String (100.0 * This.Val / This.Max, 1) & "%)",
""));
end if;
Into.Set_Color ((213, 0, 82), 255);
Fill_Rectangle (Into,
This.X + Engraving, This.Y + Engraving,
This.X + This.Width - Engraving,
This.Y + (This.Height * This.Val / This.Max) - Engraving);
end Draw;
------------------------------
-- Create_Bivariate_Ellipse --
------------------------------
function Create_Bivariate_Ellipse
(Mu : Point;
Th : Float;
Sigma : Cov2d;
Conf : Confidence := 0.95;
Curve : Boolean := True;
Axes : Boolean := True;
Points : Positive := 20) return Bivariate_Ellipse
is
begin
return
(Mu => Mu,
Th => Th,
Sigma => Sigma,
Conf => Conf,
Curve => Curve,
Axes => Axes,
Points => Points);
end Create_Bivariate_Ellipse;
----------
-- Draw --
----------
procedure Draw (This : Bivariate_Ellipse; Into : in out Drawer'Class) is
use Ada.Numerics;
use Ada.Numerics.Elementary_Functions;
use Ada.Numerics.Real_Arrays;
use Agpl.Interfaces;
function Cdf_Chi_P_Inv (P : C.Types.Double; Nu : C.Types.Double) return C.Types.Double;
pragma Import (C, Cdf_Chi_P_Inv, "gsl_cdf_chisq_Pinv");
-- This requires both GSL and BLAS
-- Nu: Degrees of freedom
K : constant Float :=
Float (Cdf_Chi_P_Inv (C.Types.Double (This.Conf), 2.0));
Sigma : constant Real_Matrix := This.Sigma * K; -- Matrix * scalar
subtype Vector2d is Point;
subtype Matrix2d is Cov2d;
Ddiag : Vector2d; -- Eigenvalues
D : Matrix2d := (others => (others => 0.0));
Dsqrt : Matrix2d;
V : Matrix2d; -- Eigenvectors
begin
Eigensystem (Sigma, Ddiag, V);
-- D should be a diagonal matrix, to match matlab.
D (1, 1) := Ddiag (1);
D (2, 2) := Ddiag (2);
for R in D'Range (1) loop
for C in D'Range (2) loop
Dsqrt (R, C) := Sqrt (abs D (R, C));
end loop;
end loop;
if This.Curve then
declare
U : Real_Matrix (1 .. 2, 1 .. This.Points);
-- 2 x Points
begin
for C in U'Range (2) loop
U (1, C) := Cos (2.0 * Pi / Float (This.Points) * Float (C - 1));
U (2, C) := Sin (2.0 * Pi / Float (This.Points) * Float (C - 1));
end loop;
declare
W : Real_Matrix := (V * Dsqrt) * U;
-- ((2x2)*(2x2))*(2xn) = (2xn)
pragma Assert (W'Length (1) = Vector2d'Length);
pragma Assert (W'Length (2) = This.Points);
begin
for C in W'Range (2) loop
-- Rotate:
declare
use Transform;
Rot : constant Transformation := Get_Rotation (This.Th);
P : constant Pose := Rot * To_Pose (W (1, C), W (2, C));
begin
W (1, C) := P (X);
W (2, C) := P (Y);
end;
-- Move to Mu
W (1, C) := This.Mu (1) + W (1, C);
W (2, C) := This.Mu (2) + W (2, C);
end loop;
-- Draw ellipse
for C1 in W'Range (2) loop
declare
C2 : Positive;
begin
if C1 = W'Last (2) then
C2 := W'First (2);
else
C2 := C1 + 1;
end if;
Into.Draw_Line (W (1, C1), W (2, C1),
W (1, C2), W (2, C2));
end;
end loop;
end;
end;
end if;
if This.Axes then
declare
use Transform;
Rot : constant Transformation := Get_Rotation (This.Th);
V1 : constant Pose := Rot * To_Pose (V (1, 1), V (2, 1));
V2 : constant Pose := Rot * To_Pose (V (1, 2), V (2, 2));
L : constant Vector2d := (Dsqrt (1, 1),
Dsqrt (2, 2));
begin
Into.Draw_Line (This.Mu (1) - L (1) * V1 (X),
This.Mu (2) - L (1) * V1 (Y),
This.Mu (1) + L (1) * V1 (X),
This.Mu (2) + L (1) * V1 (Y));
Into.Draw_Line (This.Mu (1) - L (2) * V2 (X),
This.Mu (2) - L (2) * V2 (Y),
This.Mu (1) + L (2) * V2 (X),
This.Mu (2) + L (2) * V2 (Y));
-- Into.Draw_Line (This.Mu (1) - L (1) * V (1, 1),
-- This.Mu (2) - L (1) * V (2, 1),
-- This.Mu (1) + L (1) * V (1, 1),
-- This.Mu (2) + L (1) * V (2, 1));
-- Into.Draw_Line (This.Mu (1) - L (2) * V (1, 2),
-- This.Mu (2) - L (2) * V (2, 2),
-- This.Mu (1) + L (2) * V (1, 2),
-- This.Mu (2) + L (2) * V (2, 2));
end;
end if;
end Draw;
type Robot_Type is new Drawable with record
X, Y, A, S : Float;
end record;
procedure Draw (Fig : Robot_Type; D : in out Drawer'Class) is
-- Points for a robot looking along the X axis.
H : constant Float := Fig.S / 2.0;
P1 : Cv.Point2D := ( - H, H, 1.0);
P2 : Cv.Point2D := ( - H, - H, 1.0);
P3 : Cv.Point2D := ( H, H, 1.0);
P4 : Cv.Point2D := ( H, - H, 1.0);
P5 : Cv.Point2D := ( H, 0.0, 1.0);
begin
-- Transform the points to reflect the true pose:
declare
use Transform;
T : constant Transformation :=
Get_Translation ( -Fig.X, -Fig.Y) * Get_Rotation ( -Fig.A);
begin
P1 := + (T * ( + P1));
P2 := + (T * ( + P2));
P3 := + (T * ( + P3));
P4 := + (T * ( + P4));
P5 := + (T * ( + P5));
end;
-- Arrow
D.Draw_Line (P1 (1), P1 (2), P5 (1), P5 (2));
D.Draw_Line (P2 (1), P2 (2), P5 (1), P5 (2));
-- Base
D.Draw_Line (P1 (1), P1 (2), P2 (1), P2 (2));
-- Box
D.Draw_Line (P1 (1), P1 (2), P3 (1), P3 (2));
D.Draw_Line (P2 (1), P2 (2), P4 (1), P4 (2));
D.Draw_Line (P3 (1), P3 (2), P4 (1), P4 (2));
end Draw;
-----------
-- Robot --
-----------
function Robot (X, Y, A : Float;
Size : Float := 0.5) return Drawable'Class is
begin
return Robot_Type'(X => X, Y => Y, A => A, S => Size);
end Robot;
-------------
-- Segment --
-------------
function Segment (X1, Y1, X2, Y2 : Float) return Buffer.Object is
B : Agpl.Drawing.Buffer.object;
begin
B.Draw_Line (X1, Y1, X2, Y2);
return B;
end Segment;
------------
-- Target --
------------
function Target (X, Y : Float; Size : Float := 1.0) return Buffer.Object is
B : Agpl.Drawing.Buffer.object;
D : constant Float := Size / 2.0;
begin
B.Draw_Line (X - D, Y - D, X + D, Y + D);
B.Draw_Line (X - D, Y + D, X + D, Y - D);
return B;
end Target;
--------------------
-- Simple_History --
--------------------
function Simple_History (Values : Float_Array)
return Drawing.Buffer.Object
is
B : Agpl.Drawing.Buffer.Object;
X : Float := 0.0;
begin
for I in Values'First .. Values'Last - 1 loop
B.Draw_Line (X, Values (I), X + 1.0, Values (I + 1));
X := X + 1.0;
end loop;
return B;
end Simple_History;
--------------------
-- Simple_History --
--------------------
function Simple_History (Values : Float_List)
return Drawing.Buffer.Object
is
use Float_Lists;
B : Agpl.Drawing.Buffer.object;
X : Float := 0.0;
procedure Draw (I : Cursor) is
begin
if Has_Element (Next (I)) then
B.Draw_Line (X, Element (I), X + 1.0, Element (Next (I)));
X := X + 1.0;
end if;
end Draw;
begin
Values.Iterate (Draw'Access);
return B;
end Simple_History;
--------------------
-- Simple_History --
--------------------
function Simple_History (Min, Max : Float;
Values : Float_List;
Max_Vals : Natural;
Val_RGB : Types.Rgb_Triplet := (0, 0, 255);
Axis_RGB : Types.Rgb_triplet := (0, 0, 0))
return Drawing.Buffer.Object
is
B : Agpl.Drawing.Buffer.Object;
begin
B.Set_Color (Axis_RGB, 0);
B.Draw_Line (0.0, Min, Float (Max_Vals), Min);
B.Draw_Line (0.0, Min, 0.0, Max);
B.Set_Color (Val_RGB, 0);
Simple_History (Values).Draw (B);
return B;
end Simple_History;
----------------
-- Add_Sample --
----------------
procedure Add_Sample (List : in out Float_List; Sample : Float) is
begin
List.Append (Sample);
end Add_Sample;
----------
-- Trim --
----------
procedure Trim (List : in out Float_List; Samples : Natural) is
begin
while Natural (List.Length) > Samples loop
List.Delete_First;
end loop;
end Trim;
end Agpl.Drawing.Figures;
|