Vectorization conversion issues for matrix computations

This is my first attempt to increase the speed of my already working simulation but I am running into some problems. For this post I will only focus on one file, I will take the lessons learned here and apply them to the other files myself. I do apologize for the long post but I did not want to leave anything out. I have read many post on this matter but I have not been able to complete the conversion. Any advise would be of great help. Thank you all in advance!

Problem I have working code that is written with loops. My main data matrix (flock) is a nX3X2 matrix with n know at the start of the program. I am wanting to convert the following code:

% --- loop through the flock
for i = 1 : flock_size
  agent_influence = [0,0];
  obstacle_influence = [0,0];
  % --- Calculate Influence From Agents --- %
index = calc_nearest_agent(i,flock,flock_size,range_sensing,range_collision);
  
if index == -1 
  agent_influence = [0,0];
else
  if calc_a2a_distance(flock(index,1,:),flock(i,1,:)) < range_agent_repulsion  
    agent_influence = [0,0];
  else
  agent_influence = [(flock(index,1,1)-flock(i,1,1)),(flock(index,1,2)-flock(i,1,2))];
  end
end

% --- Calculate Influence From Environment --- %
obstacle_influence = calc_obstacle_vector_range(flock(i,1,:),environment,sensing_range,flock(i,2,1),range_collision);
  
next_motion = gain_agent*agent_influence + gain_obstacle*obstacle_influence + (min_gamma+(max_gamma-min_gamma).*rand(1,1));
  
new_location(1) = next_motion(1) + flock(i,1,1); 
new_location(2) = next_motion(2) + flock(i,1,2);

v1 = [new_location(1)-flock(i,1,1),new_location(2)-flock(i,1,2)]; 
v2 = [(flock(i,1,1)+cos(deg2rad(flock(i,2,1)))-flock(i,1,1)),(flock(i,1,2)+sin(deg2rad(flock(i,2,1))))-flock(i,1,2)];

d = dot(v1,v2);
nv1 = norm(v1,2);
nv2 = norm(v2,2);
n = nv1*nv2;
if n ~= 0
  t = d/n;
  new_heading = acos(t);
  delta_heading = new_heading;
    
  if new_heading > pi/2  
    d = -1;
  else
    d = 1;
  end

    if delta_heading > max_turn
      delta_heading = max_turn;
    end

    if isreal(delta_heading)
      tmp_flock(i,2,1) = mod((flock(i,2,1)+(d*rad2deg(delta_heading))+360),360);
    else
      tmp_flock(i,2,1) = flock(i,2,1);
    end
  else
    tmp_flock(i,2,1) = flock(i,2,1);
   end
    
   if norm(next_motion,2) > max_motion
     next_motion = next_motion*(max_motion)/norm(next_motion,2);
    elseif norm(next_motion,2) == 0
      next_motion = [(flock(i,1,1) + min_motion*cos(deg2rad(flock(i,2,1)))-flock(i,1,1)),
          (flock(i,1,2)+min_motion*sin(deg2rad(flock(i,2,1)))-flock(i,1,2))]; 
    end
    
    tmp_flock(i,1,1) = tmp_flock(i,1,1) + next_motion(1); 
    tmp_flock(i,1,2) = tmp_flock(i,1,2) + next_motion(2); 
  end

End Code

The above code works but is slow. Below is my attempt to speed things up but the below code is broken in the following ways:

1. does not compute the obstacle_influence (do not worry about this part)
2. it does not take in account the range_sensing variable
3. it does not compute the correct values (namely max_motion)
4. am I on the right track for vectorizing this code

Attempt

% --- grab the flock locations
locations = flock(:,1,:);

% --- get the dimension and size of the flock
[flock_size,dims] = size(locations);

% --- reshape the location matrice
a = reshape(locations,1,flock_size,dims);
b = reshape(locations,flock_size,1,dims);

% --- compute the distances
distances = sqrt(sum((a(ones(flock_size,1),:,:) - b(:,ones(flock_size,1),:)).^2,3));

% --- convert the diagonal to NaN'a
distances(logical(eye(size(distances)))) = NaN;

% --- grab the minimum distances
[min_distances,min_index] = min(distances,[],2);

% --- grab the locations of the minimum distances
locations_of_min = locations(min_index,:);

% --- check which distances are within the repulsion radius
repulsion = range_agent_repulsion*ones(flock_size);
t = bsxfun(@ge, min_distances, repulsion);

for i = 1 : flock_size
  if t(i)
    locations_of_min(i,:) = [0,0];
  end
end

% --- reshape locations
locations = reshape(locations,flock_size,2);

% --- compute the agent influence vector
next_motion = locations-locations_of_min;
next_motion = next_motion*gain_agent;

% --- add random noise 
next_motion = next_motion + (min_gamma+(max_gamma-min_gamma).*rand(1,1));

% TODO calc the obstacle avoidance
obstacle_influence = [0,1];
obstacle_influence = ones(flock_size,2);

% --- add obstacle_influence to next_motion
next_motion = next_motion + gain_obstacle*obstacle_influence;

% --- set the new desired location for each agent
new_location = next_motion+locations;

% --- compute headings
v1 = new_location-locations; 
v2 = [(flock(:,1,1)+cos(deg2rad(flock(:,2,1)))-flock(:,1,1)),(flock(:,1,2)+sin(deg2rad(flock(:,2,1))))-flock(:,1,2)];

% --- compute the agents headings
d = dot(v1,v2,2);
nv1 = sqrt(sum(abs(v1).^2,2));
nv2 = sqrt(sum(abs(v2).^2,2));
normal = bsxfun(@times, nv1, nv2);

for i = 1 : flock_size
  if normal(i) ~= 0
    t = d(i)/normal(i);
    new_heading = acos(t);
    delta_heading = new_heading;
    
    if new_heading > pi/2  
      % --- turn right
      direction = -1;
    else
      % --- turn left
      direction = 1;
    end

    if delta_heading > max_turn
      delta_heading = max_turn;
    end

    if isreal(delta_heading)
      flock(i,2,1) = mod((flock(i,2,1)+(direction*rad2deg(delta_heading))+360),360);
    else
      flock(i,2,1) = flock(i,2,1);
    end
  else
    flock(i,2,1) = flock(i,2,1);
  end

end

normal_motion = sqrt(sum(abs(next_motion).^2,2));

in_range = normal_motion > max_motion;
next_motion(in_range) = bsxfun(@rdivide, next_motion(in_range)*max_motion, normal_motion(in_range));

% --- set the new locations of the flock members
locations = bsxfun(@plus, locations, next_motion);

flock(:,1,:) = locations;

[Mark Whirdy]

Hi Benjamin, happy to help but could you re-post a self-contained code set with all variables (flock_size,flock) defined as I get these errors everywhere so can't run the code.

Besides this, one quick observation: the "i = 1:flock_size" loop is probably unnecessary as the operation contained within isn't recursive, & so can probably be vectorized.

>> Untitled ??? Undefined function or variable 'flock'.

Error in ==> Untitled at 7
    index = calc_nearest_agent(i,flock,flock_size,range_sensing,range_collision);

[Benjamin]

Below is my current attempt in vectorizing the above code. As for the 3 parameters:

• flock is a nX3X2 array. The n represents the index of a individual flock member (agent). The second value will represent the type of data I want (location=1, heading=2, speed=3) and the last holds the actual data. Thus flock(i,1,:) = [x,y].
• max_motion is a scalar
• environment is a nX2 array of points that represent the environment

What I have fixed to this point

1. I found a computation error in setting location values to [0,0] when the distance is less than a given value, I need to set this to the current location of the agent, thus locations-location_min = [0,0]
2. I have also added checking for the sensing range into the for loop that checks for the repulsion range.

CODE function flock = micro_model_snn_plus (flock,max_motion,environment)

% --- function ret = lch_avg_model(flock,flock_size) --- 
%
% Input: 
% Output: 
%
%VECTORIZED
%
% Author: Benjamin Fine
% Creation Date: 12-16-2011
%
% ------ 

disp('') % to end the 'help' comments

% --- parameters --- %
sensing_range = 20;  % 
range_agent_repulsion = 3;  % 
range_collision = 0.5;  % 
max_gamma = 0.1;    % motion noise
min_gamma = -0.1;    % motion noise
max_turn = deg2rad(10); % in degrees
min_motion = max_motion;
gain = 1;
gain_obstacle = 9.0;
gain_agent = 0.5;

tmp_flock = flock;

% --- grab the flock locations
locations = flock(:,1,:);

% --- get the dimension and size of the flock
[flock_size,dims] = size(locations);

% --- reshape the location matrice
a = reshape(locations,1,flock_size,dims);
b = reshape(locations,flock_size,1,dims);

% --- compute the distances
distances = sqrt(sum((a(ones(flock_size,1),:,:)-b(:,ones(flock_size,1),:)).^2,3));

% --- convert the diagonal to NaN's
distances(logical(eye(size(distances)))) = NaN;

% --- grab the minimum distances
[min_distances,min_index] = min(distances,[],2);

% --- grab the locations of the minimum distances
locations_of_min = locations(min_index,:);

% --- check which distances are within the repulsion radius
repulsion = range_agent_repulsion*ones(flock_size);
r = bsxfun(@ge, min_distances, repulsion);

% --- check which distances are outside the sensing radius
sensing = sensing_range*ones(flock_size);
s = bsxfun(@le, min_distances, sensing);

for i = 1 : flock_size
  if r(i) || ~s(i)
    locations_of_min(i,:) = [0,0];
  end
end

% --- reshape locations
locations = reshape(locations,flock_size,2);

disp('LOCATIONS')
disp(locations)
disp('loc of min')
disp(locations_of_min)

% --- compute the agent influence vector
next_motion = locations-locations_of_min
pause(10)
next_motion = next_motion*gain_agent;

% --- add random noise
next_motion = next_motion + (min_gamma+(max_gamma-min_gamma).*rand(1,1));

% TODO calc the obstacle avoidance
obstacle_influence = [0,1];
obstacle_influence = ones(flock_size,2);

% --- add obstacle_influence to next_motion
next_motion = next_motion + gain_obstacle*obstacle_influence;

% --- set the new desired location for each agent
new_location = next_motion+locations;

% --- compute headings
v1 = new_location-locations; 
v2 = [(flock(:,1,1)+cos(deg2rad(flock(:,2,1)))-flock(:,1,1)),  (flock(:,1,2)+sin(deg2rad(flock(:,2,1))))-flock(:,1,2)];

% --- compute the agents headings
d = dot(v1,v2,2);
nv1 = sqrt(sum(abs(v1).^2,2));
nv2 = sqrt(sum(abs(v2).^2,2));
normal = bsxfun(@times, nv1, nv2);

for i = 1 : flock_size
  if normal(i) ~= 0
    t = d(i)/normal(i);
    new_heading = acos(t);
    delta_heading = new_heading;
    
    if new_heading > pi/2  
      % --- turn right
      direction = -1;
    else
      % --- turn left
      direction = 1;
    end

    if delta_heading > max_turn
      delta_heading = max_turn;
    end

    if isreal(delta_heading)
      flock(i,2,1) = mod((flock(i,2,1)+(direction*rad2deg(delta_heading))+360),360);
    else
      flock(i,2,1) = flock(i,2,1);
    end
  else
    flock(i,2,1) = flock(i,2,1);
  end

end

normal_motion = sqrt(sum(abs(next_motion).^2,2));

in_range = normal_motion > max_motion;
next_motion(in_range) = bsxfun(@rdivide, next_motion(in_range)*max_motion,   normal_motion(in_range));

% --- set the new locations of the flock members
locations = bsxfun(@plus, locations, next_motion);

flock(:,1,:) = locations;

end

[Benjamin]

To create a flock you can use this.

% --- random initial flock position --- %
index = 1;
for i = 1 : 10 %this is the flock_size
  % --- set the location of the flock member
  theta = rand(1,1)*(2*pi);
  r = sqrt(rand(1,1))*radius;
  
  flock(index, 1, 1) = r.*cos(theta);
       flock(index, 1, 2) = r.*sin(theta);
  
  % --- set the initial heading of the flock member (in degrees )
  flock(index, 2, 1) = 360.*rand(1,1); 
       
  % --- set the initial speed 
       flock(index, 3, 1) = 1.0;
  index = index + 1;
end

[Benjamin]

CODE WORKING NOW

So the code below works (at a visual level, I have not sat down to verify by hand) but there still seems to be room for improvements. I know the obstacle_influence fun needs help, but I will post that to a new thread once I give it a go myself first.

Code

function flock = micro_model_snn_plus (flock,max_motion,environment)

% --- function ret = lch_avg_model(flock,flock_size) --- 
%
% Input: 
% Output: 
%
%VECTORIZED
% 
% Author: Benjamin Fine
% Creation Date: 12-16-2011
%
% ------ 

disp('') % to end the 'help' comments

% --- parameters --- %
sensing_range = 20;  % 
range_agent_repulsion = 3;  % 
range_collision = 0.5;  % 
max_gamma = 0.1;    % motion noise
min_gamma = -0.1;    % motion noise
max_turn = deg2rad(10); % in degrees
min_motion = max_motion;
gain = 1;
gain_obstacle = 9.0;
gain_agent = 0.5;

tmp_flock = flock;

% --- grab the flock locations
locations = flock(:,1,:);

% --- get the dimension and size of the flock
[flock_size,dims] = size(locations);

% --- reshape the location matrice
a = reshape(locations,1,flock_size,dims);
b = reshape(locations,flock_size,1,dims);

% --- compute the distances
distances = sqrt(sum((a(ones(flock_size,1),:,:)-b(:,ones(flock_size,1),:)).^2,3));

% --- convert the diagonal to NaN's
distances(logical(eye(size(distances)))) = NaN;

% --- grab the minimum distances
[min_distances,min_index] = min(distances,[],2);

% --- grab the locations of the minimum distances
locations_of_min = locations(min_index,:);

% --- check which distances are within the repulsion radius
repulsion = range_agent_repulsion*ones(flock_size);
r = bsxfun(@ge, min_distances, repulsion);

% --- check which distances are outside the sensing radius
sensing = sensing_range*ones(flock_size);
s = bsxfun(@le, min_distances, sensing);

for i = 1 : flock_size
  if r(i) || ~s(i)
    locations_of_min(i,:) = locations(i,:);
  end
end

% --- reshape locations
locations = reshape(locations,flock_size,2);

% --- compute the agent influence vector
next_motion = locations-locations_of_min;
next_motion = next_motion*gain_agent;

% --- add random noise
next_motion = next_motion + (min_gamma+(max_gamma-min_gamma).*rand(1,1));

% TODO calc the obstacle avoidance
obstacle_influence = zeros(flock_size,2);

%tmp loop
for i = 1 : flock_size
  obstacle_influence(i,:) = calc_obstacle_vector_range(flock(i,1,:),environment,sensing_range,flock(i,2,1),range_collision);
end

% --- add obstacle_influence to next_motion
next_motion = next_motion + gain_obstacle*obstacle_influence;

% --- set the new desired location for each agent
new_location = next_motion+locations;

% --- compute headings
v1 = new_location-locations; 
v2 = [(flock(:,1,1)+cos(deg2rad(flock(:,2,1)))-flock(:,1,1)),(flock(:,1,2)+sin(deg2rad(flock(:,2,1))))-flock(:,1,2)];

% --- compute the agents headings
d = dot(v1,v2,2);
nv1 = sqrt(sum(abs(v1).^2,2));
nv2 = sqrt(sum(abs(v2).^2,2));
normal = bsxfun(@times, nv1, nv2);

for i = 1 : flock_size
  if normal(i) ~= 0
    t = d(i)/normal(i);
    new_heading = acos(t);
    delta_heading = new_heading;
    
    if new_heading > pi/2  
      % --- turn right
      direction = -1;
    else
      % --- turn left
      direction = 1;
    end

    if delta_heading > max_turn
      delta_heading = max_turn;
    end

    if isreal(delta_heading)
      flock(i,2,1) = mod((flock(i,2,1)+(direction*rad2deg(delta_heading))+360),360);
    else
      flock(i,2,1) = flock(i,2,1);
    end
  else
    flock(i,2,1) = flock(i,2,1);
  end

end

normal_motion = sqrt(sum(abs(next_motion).^2,2));

in_range = normal_motion > max_motion;
next_motion(in_range) = bsxfun(@rdivide, next_motion(in_range)*max_motion, normal_motion(in_range));

% --- set the new locations of the flock members
locations = bsxfun(@plus, locations, next_motion);

flock(:,1,:) = locations;

end