Select all points in a matrix within 30m of another point

You are looking for a spatial database like a quadtree or a kd-tree. I found two kd-tree implementations here and here, but didn't find any quadtree implementations for Matlab.

The simple solution of calculating all the distances and scanning through seems to run almost instantaneously:

lim = 1;num_trees = 1000;trees = randn(num_trees,2); %# list of trees as Nx2 matrixcur = randn(1,2); %# current point as 1x2 vectordists = hypot(trees(:,1) - cur(1), trees(:,2) - cur(2)); %# distance from all trees to current pointnearby = tree_ary((dists <= lim),:); %# find the nearby trees, pull them from the original matrix

On a 1.2 GHz machine, I can process 1 million trees (1 MTree?) in < 0.4 seconds.

Are you running the Matlab code directly on the robot? Are you using the Real-Time Workshop or something? If you need to translate this to C, you can replace hypot with sqr(trees[i].x - pos.x) + sqr(trees[i].y - pos.y), and replace the limit check with < lim^2. If you really only need to deal with 1 KTree, I don't know that it's worth your while to implement a more complicated data structure.

You can transform you cartesian coordinates into polar coordinates with CART2POL. Then selecting points inside certain radius will be strait-forward.

[THETA,RHO] = cart2pol(X-X0,Y-Y0);selected =  RHO < 30;

where X0, Y0 are coordinates of the current location.