-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathballMovingManager.m
63 lines (55 loc) · 1.71 KB
/
ballMovingManager.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
function [fastMoving, saveDir, BPosHX, BPosHY] = ballMovingManager(B)
persistent oldFastMoving;
persistent oldSaveDir;
persistent hX; %history of ball coordinate X
persistent hY; %history of ball coordinate Y
minBallSpeed = 50;
hLen = 5; %historyLength
if isempty(oldFastMoving)
oldFastMoving = false;
end
if isempty(oldSaveDir)
oldSaveDir = true;
end
global outBuffer;
if (B.I)
if (isempty(hX) || isempty(hY) || numel(hX) < hLen || numel(hY) < hLen)
hX = zeros(1, hLen);
hY = zeros(1, hLen);
for i = 1: hLen
hX(i) = B.x;
hY(i) = B.y;
end
else
for i = 1: hLen - 1
hX(i) = hX(i + 1);
hY(i) = hY(i + 1);
end
hX(hLen) = B.x;
hY(hLen) = B.y;
end
ballMovement = B.z - [hX(hLen - 3), hY(hLen - 3)];
fastMoving = norm(ballMovement) > minBallSpeed;
if (fastMoving)
%çàäà¸ò äîïóñòèìûå çíà÷åíèÿ êîñèíóñà óãëà ìåæäó òåêóùèì è
%ïðåäûäóùèì íàïðàâëåíèÿìè äâèæåíèÿ ìÿ÷à
sensivity = 0.8;
curDir = B.z - [hX(hLen - 1), hY(hLen - 1)];
prevDir = [hX(hLen - 1) - hX(hLen - 2), hY(hLen - 1) - hY(hLen - 2)];
saveDir = getCos(curDir, prevDir) >= sensivity;
else
saveDir = true;
end
else
saveDir = oldSaveDir;
fastMoving = oldFastMoving;
end
oldFastMoving = fastMoving;
oldSaveDir = saveDir;
outBuffer(2) = fastMoving;
BPosHX = hX;
BPosHY = hY;
end
function res = getCos(U, V)
res = scalMult(U, V) / (norm(U) * norm(V));
end