image thumbnail
from Drive Magnetic Micro-Robots Through a 2D Vascular Network by Aaron Becker
Using the mouse or keyboard, drive n robots through a 2D vascular structure to goal positions

leafNavigate.m
function leafNavigate
% drive some robots through a 2D vascular structure using the mouse or keyboard. 
% The robots are quite simple -- they all move in the direction you command
% until they hit an obstacle or another robot. Similar games include
% Richochet Robots and Atomix, but here ALL the robots move at each
% command.  This simulates magnetotactic bacteria, or magnetized
% tetrahymena pyriformis.  This example appears in 
% "Reconfiguring Massive Particle Swarms with Limited, Global Control",
% by Aaron Becker, Erik Demaine, Sndor Fekete, Golnaz Habibi, and James McLurkin
% 9th International Symposium on Algorithms and Experiments for Sensor Systems,
% Wireless Networks and Distributed Robotics September 5-6, 2013. (Fig. 1)
%
% Author: Aaron Becker
%
% video: http://www.youtube.com/watch?v=eExZO0HrWRQ
% http://www.youtube.com/user/aabecker5
%
%Now uses 8-point connectivity by mapping
% q,w,e
% a,  d
% z,x,c
% to the 8 directions.  You can also use the keyboard arrows to move in 8
% directions, or use the mouse to follow arbitrary angles (uses Bresenham algorithm for this).
global G MAKE_MOVIE RobotPts
clear all
clc
format compact
MAKE_MOVIE = false;
PLAY_GAME = true;  % if true, sets goal positions (Making this a very hard game)
G.myPath = [];
G.fig = figure(1);
G.numCommands = 0;
G.totalMoves = 0;
%G.myPath = [ -3.0983    2.0386    3.0633   -2.4755    3.0712   -3.1239    2.8463   -2.6887    2.8358    2.7922    2.9520    2.0646    1.9245    1.8796, 1.5522    2.2420    2.2118    1.0457    2.0230   -0.8220   -2.4420   -2.5104   -1.8643   -2.0579];
% This path takes 5 robots to different places.
%G.myPath =  [    2.7776   -2.1360    2.9686    2.8572    1.3480    0.8497    2.0207    1.6868    1.4384    1.8775    1.7184    2.0832    2.5068    1.8458,    2.1358    1.5495    2.1489   -2.9480   -2.6605   -2.1474    3.0040   -2.7140   -0.3271   -0.1214   -2.5512   -1.8684   -1.6009   -2.5029,   -1.9609   -1.7520   -1.5784   -2.1006   -1.3003   -2.4299   -1.6653   -2.0752   -1.7552   -2.1869   -0.9842   -2.3105   -1.5797    3.1207,   -2.7743    3.1332    1.9862    1.0648    1.6067    2.1173   -2.4847    3.0700   -2.9086    2.1306   -2.8893    2.9367   -2.7982    2.8984,   -2.8207    3.0039    2.8442   -2.7527    2.7148   -3.0317    2.7757   -2.9472    2.9100   -3.0928    2.7895    2.2131    1.7643    2.3571,    2.0234    1.7658    2.2824    1.6140   -0.9257   -2.0081    0.2143    0.5685   -0.8509    0.4710   -1.6509   -0.7825   -0.0675   -0.3420,   -0.0394   -2.2007   -1.2081    1.4950   -2.4113   -2.3222   -1.7498    3.1412   -2.4062   -2.6104    2.8948   -0.0031    2.7551    2.8501,   -1.3935   -0.5697   -0.5232   -0.1789   -2.5100    1.7367   -2.1786   -1.1867   -3.1329    1.9882   -0.8246   -2.9432   -2.2731   -2.5727,   -2.3857   -3.0260   -2.3039    3.1257    2.4746    1.8279    2.5034    0.2100    1.1198    1.6610    0.5301    1.2663   -3.0725   -2.2845,   -1.3098   -0.5017   -0.2175    0.7129   -2.3439   -0.2566   -0.1473   -1.2827    2.8342   -2.7624    2.8878   -2.7576   -0.1611   -3.0500,    2.9760   -1.8528   -1.3044   -0.4422    2.9528   -2.9263   -2.7526    2.4208    2.9268    2.1611    2.4117    1.8427    2.5548   -3.1364,   -2.7663    2.3894    1.8298    2.9560   -2.8178    2.3459    2.5819    2.3893    3.0108    2.3557    2.6253    1.8691    0.1800   -0.1089,   -0.3680    1.3030    1.0685    0.9978    0.8664    1.8279    0.9660    1.5173    1.0362    1.8928    1.6683    1.0339    2.5420    2.5836];
imgname = 'leafBW.png';
[BW,map] = imread(imgname); %#ok<NASGU>
G.obstacle_pos = flipud(1-BW');
%G.obstacle_pos = zeros(size(G.obstacle_pos));
%create vector of robots and draw them.  A robot vector consists of an xy
%position and a color.
RobotPts =    [765,   336,1,1;
                770 ,  329,2,2;
                764 ,  324,3,3;
                778 ,  322,4,4;
                769 ,  311,5,5;
                ];
G.EMPTY = 0;
G.OBST = 1;
G.maxX = size(G.obstacle_pos,2);
G.maxY = size(G.obstacle_pos,1);

numRobots = size(RobotPts,1);
clf
set(G.fig,'Units','normalized','outerposition',[0 0 1 1],'NumberTitle','off');%'MenuBar','none',

colors = hsv(numel(unique(RobotPts(:,4))));
G.colormap = [  1,1,1; %Empty = white
    0,0,0; %obstacle
    ];

colormap(G.colormap);
G.axis=imagesc(G.obstacle_pos);

G.path = [];
set(gca,'box','off','xTick',[],'ytick',[],'ydir','normal');%,'Visible','off');
%set(G.axis,'edgealpha',.08)
axis equal
axis tight
G.title = title('press arrows keys to move');
%ginput(5)
%screen_size = get(0, 'ScreenSize');
%set(f1, 'Position', [0 0 screen_size(3) screen_size(4) ] );
rad = 4;
G.hRobots = zeros(1, numRobots);
G.rLines = zeros(1, numRobots);
for hi = 1: numRobots
    G.rLines(hi) = line(RobotPts(hi,1),RobotPts(hi,2),'Color', colors(RobotPts(hi,4),:),'linewidth',2);
    G.hRobots(hi) =  rectangle('Position',[RobotPts(hi,1)-rad/2,RobotPts(hi,2)-rad/2,rad,rad],'Curvature',[1,1],'FaceColor',colors(RobotPts(hi,4),:));
end
sc = 50;
G.VectorLineX = sc*[-3, -3, -1,-1,0,-1,  -1,  -3, -3];
G.VectorLineY = sc*[ 0,1/2,1/2, 1,0,-1,-1/2,-1/2,0]/2;
G.VectorLineW = patch(G.VectorLineX,G.VectorLineY,'g','linewidth',4,'FaceAlpha',0.5,'EdgeAlpha',0.8,'EdgeColor','g');
%G.VectorLineB = line(G.VectorLineX,G.VectorLineY,'color','k','linewidth',2);

if PLAY_GAME
    title({'Drive the robots to their targets using your mouse or the keyboard';'Arrow keys or [q,w,e,a,d,z,x,c]'})
    numGoals = 0;
    G.goals =[];
    while numGoals < numRobots
        pos = [floor(rand(1)*G.maxX+1),floor(rand(1)*G.maxY+1)] ; 
        if G.obstacle_pos(pos(2),pos(1)) == 0
            G.goals(numGoals+1,:) = pos;
            numGoals = numGoals+1;
        end
    end
    G.radG = 3*rad;
    for i = 1:numRobots
        rectangle('Position',[G.goals(i,1),G.goals(i,2),1,1],'linewidth',2,'Curvature',[1,1],'FaceColor',colors(i,:),'EdgeColor',colors(i,:));
        rectangle('Position',[G.goals(i,1)-G.radG/2,G.goals(i,2)-G.radG/2,G.radG,G.radG],'linewidth',2,'Curvature',[1,1],'FaceColor','none','EdgeColor',colors(i,:));
    end
    
end

if MAKE_MOVIE
    set(G.fig ,'Name','Massive Control','color',[1,1,1]); %#ok<UNRCH>
else
    set(G.fig,'KeyPressFcn',@keyhandler,'Name','Massive Control')
end
set(G.fig,'WindowButtonDownFcn',@mousehandler, 'WindowButtonMotionFcn', @mousemove)


for i = 1:numel(G.myPath)
    %implement a pre-programmed route
    moveAlongAngle(G.myPath(i));
end

    function keyhandler(src,evnt) %#ok<INUSL>
        if strcmp(evnt.Key,'s')
            imwrite(flipud(get(G.axis,'CData')+1), G.colormap, 'Aleafpic.png');
        else
            moveto(evnt.Key)
        end
    end


    function moveto(key)
        % Maps keypresses to moving pixels
        step = [0,0];
        if strcmp(key,'leftarrow') || strcmp(key,'a') %-x
            RobotPts = sortrows(RobotPts,1);
            step = -[1,0];
        elseif strcmp(key,'rightarrow')|| strcmp(key,'d') %+x
            RobotPts = sortrows(RobotPts,-1);
            step = [1,0];
        elseif strcmp(key,'uparrow')|| strcmp(key,'w') %+y
            RobotPts = sortrows(RobotPts,-2);
            step = [0,1];
        elseif strcmp(key,'downarrow')|| strcmp(key,'x') %-y
            RobotPts = sortrows(RobotPts,2);
            step = -[0,1];
        elseif strcmp(key,'leftarrow') || strcmp(key,'q') %-x,+y
            RobotPts = sortrows(RobotPts,1);
            step = [-1,+1];
        elseif strcmp(key,'rightarrow')|| strcmp(key,'e') %+x,+y
            RobotPts = sortrows(RobotPts,-1);
            step = [1,1];
        elseif strcmp(key,'uparrow')|| strcmp(key,'c') %+x,-y
            RobotPts = sortrows(RobotPts,-1);
            step = [1,-1];
        elseif strcmp(key,'downarrow')|| strcmp(key,'z') %-x-y
            RobotPts = sortrows(RobotPts,1);
            step = [-1,-1];
        end
        % implement the move on every robot
        movement = 0;
        for ni = 1:size(RobotPts,1)
            stVal = RobotPts(ni,1:2);
            desVal = RobotPts(ni,1:2)+step;
            
            % move there if no robot in the way and space is free
            while  ~ismember(desVal,RobotPts(:,1:2),'rows')  ...
                    && desVal(1) >0 && desVal(1) <= G.maxX && desVal(2) >0 && desVal(2) <= G.maxY ...
                    && G.obstacle_pos( desVal(2),desVal(1) )==0
                RobotPts(ni,1:2) = desVal;
                desVal = RobotPts(ni,1:2)+step;
                movement = 1;
            end
            if ~isequal( stVal, RobotPts(ni,1:2) )
                set(G.hRobots(RobotPts(ni,3)),'Position',[RobotPts(ni,1)-rad/2,RobotPts(ni,2)-rad/2,rad,rad]);
            end
            xD = [get(G.rLines(RobotPts(ni,3)),'Xdata'),RobotPts(ni,1)];
            yD = [get(G.rLines(RobotPts(ni,3)),'Ydata'),RobotPts(ni,2)];
            set(G.rLines(RobotPts(ni,3)),'Xdata',xD,'Ydata',yD);
        end
        if movement
            G.path(end+1) = atan2(step(2),step(1));
            display(G.path)
            calculateSuccess();
        end
    end

    function calculateSuccess()
        if PLAY_GAME
            rAtGoal = 0;
            for ni = 1:numRobots
                if sum((RobotPts(ni,1:2) - G.goals(RobotPts(ni,3),1:2)).^2)< G.radG^2
                    rAtGoal = rAtGoal+1;
                end
            end
            title([num2str(rAtGoal),' robots of ', num2str(numRobots),' at goal']);
        end
    end
                

    function mousemove(~,~,~)
        %draw a line showing where the robot will move
        pt = get(gca,'Currentpoint');
        xMax = size(G.obstacle_pos,2);
        yMax = size(G.obstacle_pos,1);
        if pt(1,1)>=0 && pt(1,1)<=xMax && pt(1,2)>=0 && pt(1,2)<=yMax
            %x = [xMax/2, pt(1,1)];
            %y = [yMax/2, pt(1,2)];
            angle = atan2( pt(1,2) - yMax/2, pt(1,1) - xMax/2);
            x =pt(1,1) + cos(angle)*G.VectorLineX-sin(angle)*G.VectorLineY;
            y =pt(1,2) + sin(angle)*G.VectorLineX+cos(angle)*G.VectorLineY;
            set(G.VectorLineW,'Xdata', [x,xMax/2] ,'YData',[y,yMax/2]);
            %set(G.VectorLineB,'Xdata', [xMax/2,x] ,'YData',[yMax/2,y]);
        else
            set(G.VectorLineW,'Xdata', 0 ,'YData',0);
            %set(G.VectorLineB,'Xdata', 0 ,'YData',0);
        end
        
    end

    function  mousehandler(~,~,~)
        pt = get(gca,'Currentpoint');
        xMax = size(G.obstacle_pos,2);
        yMax = size(G.obstacle_pos,1);
        %if pt(1,1)>=0 && pt(1,1)<=xMax && pt(1,2)>=0 && pt(1,2)<=yMax
        if xMax/2 ==  pt(1,1) && yMax/2 == pt(1,2)
            return;
        end
        angle = atan2( pt(1,2) - yMax/2, pt(1,1) - xMax/2);
        moveAlongAngle(angle)
    end

    function moveAlongAngle(angle)
        xMax = size(G.obstacle_pos,2);
        yMax = size(G.obstacle_pos,1);
        length = xMax+yMax;
        [yP,xP] = bresenham(angle, length);
        
        %sort the robots
        if abs(angle) <= pi/4
            RobotPts = sortrows(RobotPts,1);
        elseif pi/4 < angle && angle<= 3*pi/4
            RobotPts = sortrows(RobotPts,2);
        elseif 3*pi/4 <= abs(angle)
            RobotPts = sortrows(RobotPts,-1);
        elseif -pi/4 > abs(angle) && abs(angle) >= -3*pi/4
            RobotPts = sortrows(RobotPts,-2);
        end
        
        % implement the move on every robot
        movement = 0;
        for ni = 1:size(RobotPts,1)
            stVal = RobotPts(ni,1:2);
            step = [xP(1),yP(2)];
            desVal = RobotPts(ni,1:2)+step;
            count = 1;
            % move there if no robot in the way and space is free
            while  ~ismember(desVal,RobotPts(:,1:2),'rows')  ...
                    && desVal(1) >0 && desVal(1) <= G.maxX && desVal(2) >0 && desVal(2) <= G.maxY ...
                    && G.obstacle_pos( desVal(2),desVal(1) )==0 ...
                    && count < numel(xP)-1
                RobotPts(ni,1:2) = desVal;
                movement = 1;
                count = count+1;
                step = [xP(count),yP(count)];
                desVal = RobotPts(ni,1:2)+step;
            end
            if ~isequal( stVal, RobotPts(ni,1:2) )
                set(G.hRobots(RobotPts(ni,3)),'Position',[RobotPts(ni,1)-rad/2,RobotPts(ni,2)-rad/2,rad,rad]);
            end
            xD = [get(G.rLines(RobotPts(ni,3)),'Xdata'),RobotPts(ni,1)];
            yD = [get(G.rLines(RobotPts(ni,3)),'Ydata'),RobotPts(ni,2)];
            set(G.rLines(RobotPts(ni,3)),'Xdata',xD,'Ydata',yD);
        end
        if movement
            G.path(end+1) = angle;
            display(G.path)
            calculateSuccess()
        end
    end

    function [xP,yP] = bresenham(angle, length)
        x = [0,  round(cos(angle)*length)];
        y = [0,  round(sin(angle)*length)];
        steep = (abs(y(2)-y(1)) > abs(x(2)-x(1)));
        if steep,
            [x,y] = swap(x,y);
        end
        signCh = 0;
        if x(1)>x(2),
            [x(1),x(2)] = swap(x(1),x(2));
            signCh = 1;
        end
        
        delx = x(2)-x(1);
        dely = abs(y(2)-y(1));
        error = 0;
        
        if signCh,  x_n = -1; else x_n = 1;          end
        if y(1) < y(2), ystep = 1; else ystep = -1; end
        xP = zeros(1,delx);
        yP = zeros(1,delx);
        for n = 1:delx
            y_n = 0;
            error = error + dely;
            if bitshift(error,1) >= delx, % same as -> if 2*error >= delx,
                y_n = ystep;
                error = error - delx;
            end
            
            if steep,
                xP(n) = x_n;
                yP(n) = y_n;
            else
                xP(n) = y_n;
                yP(n) = x_n;
            end
        end
    end
    function [q,r] = swap(s,t)
        % function SWAP
        q = t; r = s;
    end
end

Contact us