MLscripts/GoalKeeper.m at master · robocup-ssl-russia/MLscripts · GitHub

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
%goalkeeper who defends gates using circle trajectory(tactic: long distance goalkeeper)
%äîåõàòü â òî÷êó À, ðàçâåðíóòüñÿ ê ìÿ÷ó
%agent - ðîáîò, Â - ìÿ÷, G - öåíòð âîðîò
%RP.Blue(1).rul = GoalKeeperNew(agent, B, G, V);

function rul = GoalKeeper(agent, B, G, V)
    v = 400; % îêðåñòíîñòü âîðîò, èõ øèðèíà, ðàäèóñ îêðóæíîñòè
    eps = 1e-3;
    radius = 5;

    persistent tmp;
    if (isempty(tmp))
        tmp = [B.x B.y];
    end
    %åñëè ìÿ÷ ïîäâèíóëñÿ
    if (sqrt((tmp(1) - B.x) ^ 2 + (tmp(2) - B.y) ^ 2) >= radius) && (V(1) * (B.x - G(1)) + V(2) * (B.y - G(2)) >= 0)

        %íàõîäèì êîîðäèíàòû òî÷êè, ëåæàùåé íà ïðÿìîé è áëèæàéøåé ê öåíòðó îêðóæíîñòè
        %öåíòð êîîðäèíàò ñìåùåí ê öåíòðó âîðîò
        a = -tmp(2) + B.y;
        b = tmp(1) - B.x;
        c = (B.x - G(1))*(tmp(2) - G(2)) - (tmp(1) - G(1))*(B.y - G(2));
        d = a^2+b^2;
        point = [-a*c/d+G(1), -b*c/d+G(2)];
        %êîîðäèíàòû òî÷êè óæå â îáû÷íûõ êîîðäèíàòàõ

        %åñëè îíà âíóòðè êðóãà, ò.å. ïðåäïîëîãàåìàÿ òðàåêòîðèÿ ïåðåñåêëà
        %îêðóæíîñü, è íàäî äâèãàòüñÿ
        r_to_point2 = c ^ 2 / d;
        if r_to_point2 + eps < v ^ 2
            %äëèíà âåêòîðà, êîòîðûé íàäî ïðèáàâèòü ê point, ÷òîáû ïîëó÷èòü
            %òî÷êó ïåðåñå÷åíèÿ
            length = sqrt(v ^ 2 - r_to_point2);
            % óãîë íàêëîíà âåêòîðà
            %{
            angle = atan(tmp(1) - B.x, tmp(2) - B.y);
            X = point(1) + length * cos(angle);
            Y = point(2) + length * sin(angle);
            %}
            dir = [B.x - tmp(1), B.y - tmp(2)];
            dir = dir / sqrt(dir(1) ^ 2 + dir(2) ^ 2);
            fpnt1 = point + length * dir;
            fpnt2 = point - length * dir;

            if ((fpnt1(1) - B.x) ^ 2 + (fpnt1(2) - B.y) ^ 2 > (fpnt2(1) - B.x) ^ 2 + (fpnt2(2) - B.y) ^ 2)
                fpnt1 = fpnt2;
            end

            rul = MoveToPointForGoalKeeper(agent, fpnt1, B);
        else
            finalPoint = [B.x-G(1), B.y-G(2)];
            finalPoint = G + finalPoint*v/sqrt(finalPoint(1)^2+finalPoint(2)^2);
            rul = MoveToPointForGoalKeeper(agent, finalPoint, B);
        end
    else
        rul = Crul(0, 0, 0, 0, 0);
    end


    tmp = [B.x, B.y];
end