Optimizing collision detection using a quadtree - determining adjacent cells under periodic conditions?

5 views (last 30 days)
In my simulations, I currently compute pairwise distances for collision detection between (potentially overlapping, same-size) circles. I want to optimize my code, and have been trying to use quadtrees as a starting point. Additionally, I want to be able to write down a list of each cell's neighbors. I have found code that does this, but at boundaries/corners does not wrap-around. It is provided below. Can anyone give me some advice on how to adjust this code? Thank you!
function [ind,bx,by,Nb,lx,ly] = quadtree(x,y,s,n0)
% QUADTREE Recursive division of a 2-dimensional set.
% [IND,BX,BY,NB,LX,LY] = QUADTREE(X,Y,S,N0)
% Performs recursive tree-like division of a set
% of points with coordinates X,Y.
% S is binary mask showing which points of a set
% are to be counted. N0 is maximum permissible
% number of "counted" points in the elementary
% block.
% Returns vector IND of the same size as X, Y
% showing which region each point of a set belongs
% to; binary matrices BX, BY where each row shows
% "binary address" of each region.
% Also returns "Adjacency matrix" NB which is 1 if
% i and j regions are neighbours and 0 otherwise;
% and matrices of limits for each region, LX and LY
% so that PLOT(LX(:,[1 2 2 1 1])',LY(:,[1 1 2 2 1])')
% plots the boundaries of all regions.
% Copyright (c) 1995 by Kirill K. Pankratov
% kirill@plume.mit.edu
% 01/30/95
% Default for maximal block size
n0_dflt = 10;
% Handle input ..............................
if nargin < 2
error(' Not enough input arguments.')
end
if nargin<4, n0 = n0_dflt; end
if nargin<3, s = []; end
if isempty(s), s = ones(size(x)); end
% If no need to divide ......................
if length(x(find(s))) <= n0
bx = []; by = []; Nb = [];
lx = []; ly = [];
ind = ones(size(x));
return
end
% Calculate limits ................
lim = [min(x) max(x) min(y) max(y)];
% Recursively construct quadtree ............
[ind,bx,by] = qtree0(x,y,s,lim,n0);
bx = bx(:,1:size(bx,2)-1);
by = by(:,1:size(by,2)-1);
% Compose "adjacency matrix" ................
szb = size(bx);
ob = ones(1,szb(1));
pow = 2.^(0:szb(2)-1);
pow = flipud(pow');
% "Lower" and "upper" bounds for trees
bxmax = ceil(bx)*pow;
bxmin = floor(bx)*pow;
bymax = ceil(by)*pow;
bymin = floor(by)*pow;
% "Physical" limits of each regions
lx = [bxmin bxmax+1];
ly = [bymin bymax+1];
lx = lim(1)+lx*(lim(2)-lim(1))/pow(1)/2;
ly = lim(3)+ly*(lim(4)-lim(3))/pow(1)/2;
B0 = bxmin(:,ob);
B1 = bxmax(:,ob);
Nb = (B0'-B1<=1) & (B1'-B0>=-1);
B0 = bymin(:,ob);
B1 = bymax(:,ob);
Nb = Nb & (B0'-B1<=1) & (B1'-B0>=-1);
function [ind,bx,by] = qtree0(x,y,s,lim,n0)
% QTREE0 Primitive for QUADTREE.
% [IND,BX,BY] = QTREE0(X,Y,S,LIM,N0)
% Copyright (c) 1995 by Kirill K. Pankratov
% kirill@plume.mit.edu
% 01/26/95
% Divide and make further recursion if necessary
N = length(find(s));
% If not to divide further .....................
ind = ones(size(x));
if N <= n0
bx = .5; by = .5;
return
end
% Further division is needed ...................
% Calculate middle of the current range
x_mid = (lim(2)+lim(1))/2;
y_mid = (lim(4)+lim(3))/2;
% Branch for current level
bx0 = [0 0 1 1]';
by0 = [0 1 0 1]';
n1 = find( x<=x_mid & y<=y_mid ); % Lower-left
lm = [lim(1) x_mid lim(3) y_mid];
[ind1,bx1,by1] = qtree0(x(n1),y(n1),s(n1),lm,n0);
n2 = find( x<x_mid & y>y_mid ); % Upper-left
lm = [lim(1) x_mid y_mid lim(4)];
[ind2,bx2,by2] = qtree0(x(n2),y(n2),s(n2),lm,n0);
n3 = find( x>x_mid & y<=y_mid ); % Lower-right
lm = [x_mid lim(2) lim(3) y_mid];
[ind3,bx3,by3] = qtree0(x(n3),y(n3),s(n3),lm,n0);
n4 = find( x>x_mid & y>y_mid ); % Upper-right
lm = [x_mid lim(2) y_mid lim(4)];
[ind4,bx4,by4] = qtree0(x(n4),y(n4),s(n4),lm,n0);
% Make composite binary "tree matrices"
szB = [size(bx1); size(bx2); size(bx3); size(bx4)];
szv = cumsum(szB(:,1));
szh = max(szB(:,2))+1;
% Initialize output matrices
bx = .5*ones(szv(4),szh);
by = bx;
% Fill binary matrices ..................
nnv = (1:szv(1))'; nnh = 2:szB(1,2)+1;
bx(nnv,nnh) = bx1; by(nnv,nnh) = by1;
bx(nnv,1) = bx0(1)*ones(size(nnv));
by(nnv,1) = by0(1)*ones(size(nnv));
nnv = (szv(1)+1:szv(2))'; nnh = 2:szB(2,2)+1;
bx(nnv,nnh) = bx2; by(nnv,nnh) = by2;
bx(nnv,1) = bx0(2)*ones(size(nnv));
by(nnv,1) = by0(2)*ones(size(nnv));
nnv = (szv(2)+1:szv(3))'; nnh = 2:szB(3,2)+1;
bx(nnv,nnh) = bx3; by(nnv,nnh) = by3;
bx(nnv,1) = bx0(3)*ones(size(nnv));
by(nnv,1) = by0(3)*ones(size(nnv));
nnv = (szv(3)+1:szv(4))'; nnh = 2:szB(4,2)+1;
bx(nnv,nnh) = bx4; by(nnv,nnh) = by4;
bx(nnv,1) = bx0(4)*ones(size(nnv));
by(nnv,1) = by0(4)*ones(size(nnv));
% Calculate indices .....................
ind2 = ind2+szv(1);
ind3 = ind3+szv(2);
ind4 = ind4+szv(3);
ind(n1) = ind1;
ind(n2) = ind2;
ind(n3) = ind3;
ind(n4) = ind4;

Answers (0)

Categories

Find more on Statistics and Machine Learning Toolbox in Help Center and File Exchange

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!