MLscripts/MoveToWithFastBuildPath.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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
function rul = MoveToWithFastBuildPath(agent, aimPoint, aimVicinity, obstacles)
    step = 80;
    NormalSpeed = 50;
    infinity = 1000000;
    minSpeed = 10;
    minMovement = 100;
    coef = 2/750;


    [obst, ~] = getNearestObstacle(agent.z, obstacles);
    agentPos = agent.z;

    %{
        åñëè ðîáîò îêàçàëñÿ âíóòðè ïðåïÿòñâèÿ
        (íàïðèìåð èç-çà ïîãðåøíîñòåé êàìåðû èëè ïðîñòî íåìíîãî íå âûðóëèë)
        ÷òîáû àëãîðèòì ðàñ÷¸òà ìàðøðóòà ñðàáîòàë íîðìàëüíî, ñ÷èòàåì ìàðøðóò
        íå îò ðîáîòà, à îò òî÷êè êîòîðàÿ íàõîäèòñÿ ðÿäîì ñ ïðåïÿòñòâèåì
    %}
    if isObstaclePoint(agentPos, [obstacles(obst, 1), obstacles(obst, 2)], obstacles(obst, 3))
        agentPos = getPointOutOfObstacle(agentPos, [obstacles(obst, 1), obstacles(obst, 2)], obstacles(obst, 3), 10);
    end

    if r_dist_points(agent.z, aimPoint) < aimVicinity
        rul = Crul(0, 0, 0, 0, 0);
        %{
    elseif ~CheckIntersect([agent.x, agent.y], aimPoint, obstacles)
        rul = MoveToLinear(agent, aimPoint, 0, NormalSpeed, 0);
        %}
    else
        % ñòðîèòñÿ äâà ïóòè (îáõîäÿò ïðÿïÿòñâèÿ ñ ðàçëè÷íûõ ñòîðîí)
        firstPath = fastBuildPath(agentPos, aimPoint, obstacles, -step, 0);
        secondPath = fastBuildPath(agentPos, aimPoint, obstacles, step, 0);

        firstLength = 0;
        if (~firstPath.isEmpty())
            prevPnt = firstPath.pop();
            firstPoint = firstPath.getFirst();
            % âû÷èñëÿåì äëèíó ïåðâîãî ìàðøðóòà
            while ~firstPath.isEmpty()
                firstLength = firstLength + r_dist_points(prevPnt, firstPath.getFirst());
                prevPnt = firstPath.pop();
            end
        else
            firstLength = infinity;
        end

        secondLength = 0;
        if (~secondPath.isEmpty())
            prevPnt = secondPath.pop();
            secondPoint = secondPath.getFirst();
            % âû÷èñëÿåì äëèíó âòîðîãî ìàøðóòà
            while ~secondPath.isEmpty()
                secondLength = secondLength + r_dist_points(prevPnt, secondPath.getFirst());
                prevPnt = secondPath.pop();
            end
        else
            secondLength = infinity;
        end

        if (firstLength ~= infinity || secondLength ~= infinity)
            if (firstLength < secondLength)
                point = [firstPoint(1), firstPoint(2)];
            else
                point = [secondPoint(1), secondPoint(2)];
            end
            rul = MoveToLinear(agent, point, 0, NormalSpeed, 0);
        else
            rul = Crul(0, 0, 0, 0, 0);
        end
    end
end

function [res] = isObstaclePoint(point, obstCenter, obstRadius)
    res = r_dist_points(point, obstCenter) < obstRadius;
end

function [res] = getPointOutOfObstacle(point, obstCenter, obstRadius, step)
    vect = point - obstCenter;
    vect = vect / sqrt(vect(1) ^ 2 + vect(2) ^ 2) * (step + obstRadius * sign(step));
    res = obstCenter + vect;
end