Steer-behavior-MATLAB/steer_boundaries.m at master · miamor/Steer-behavior-MATLAB · 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
function steer = steer_boundaries(vehicles, vhl)
  global boundaryWidth;

  v_pos = vehicles(vhl,1:3);
  v_vel = vehicles(vhl,4:6);
  v_acc = vehicles(vhl,7:9);
  v_maxspeed = vehicles(vhl,10);
  v_maxforce = vehicles(vhl,11);

  desired = [0 0 0];
  steer = [0 0 0];

  w = boundaryWidth;
  h = boundaryWidth;

%{
  if (abs(v_pos(1)) < w && abs(v_pos(2)) < h)
    desired = [v_maxspeed v_maxspeed v_pos(3)];
  end
%}
  if (v_pos(1) >= w)
    if (v_pos(2) >= h)
      desired = [-v_maxspeed -v_maxspeed v_pos(3)];
    elseif (v_pos(2) <= -h)
      desired = [-v_maxspeed v_maxspeed v_pos(3)];
    else
      desired = [-v_maxspeed v_pos(2) v_pos(3)];
    end
  elseif (v_pos(1) <= -w)
    if (v_pos(2) >= h)
      desired = [v_maxspeed -v_maxspeed v_pos(3)];
    elseif (v_pos(2) <= -h)
      desired = [v_maxspeed v_maxspeed v_pos(3)];
    else
      desired = [v_maxspeed -v_pos(2) v_pos(3)];
    end
  else
    if (v_pos(2) >= h)
      desired = [v_pos(1) -v_maxspeed v_pos(3)];
    elseif (v_pos(2) <= -h)
      desired = [v_pos(1) v_maxspeed v_pos(3)];
    end
  end


  %if (any(desired))  % if desired not 0-matrix
    desired = normalize(desired);
    desired = desired*v_maxspeed;

    steer = desired - v_vel;
    steer = setLimit(desired, v_maxforce);
  %end

end