function [HvelsLeft, VvelsLeft, HvelsRight, VvelsRight] = split_centroids(Centroids) % given a list of three centroids, figure out which is which % once initially determined, continue tracking them by always % choosing as the next corresponding the centroid the one that % is closest to the one under consideration curCentroids = Centroids(1:3, :); [CMin IMin] = min(curCentroids(1:3, 1)); %left hand [CMax IMax] = max(curCentroids(1:3, 1)); %right hand LastLeft = curCentroids(IMin, :); LastRight = curCentroids(IMax, :); [rows cols] = size(Centroids); HvelsLeft = []; VvelsLeft = []; HvelsRight = []; VvelsRight = []; for i = 4:3:rows %start at 4, step by 3, go to cols curCentroids = Centroids(i:(i+2), :); distancesLeft = [LastLeft;LastLeft;LastLeft] - curCentroids; distancesRight = [LastRight;LastRight;LastRight] - curCentroids; distsL = sqrt((distancesLeft(:,1).^2) + (distancesRight(:,2).^2)); distsR = sqrt((distancesRight(:,1).^2) + (distancesRight(:,2).^2)); [LMin iL] = min(distsL); [RMin iR] = min(distsR); CurLeft = curCentroids(iL, :); CurRight = curCentroids(iR, :); HvelsLeft = [HvelsLeft, CurLeft(1) - LastLeft(1)]; VvelsLeft = [VvelsLeft, CurLeft(2) - LastLeft(2)]; HvelsRight = [HvelsRight, CurRight(1) - LastRight(1)]; VvelsRight = [VvelsRight, CurRight(2) - LastRight(2)]; LastLeft = CurLeft; LastRight = CurRight; end