極座標で表された2点間の距離を求める

極座標 x=rcosθ，y=rsinθで表された2点間の距離を出したいです．

forループで2642秒まで1秒ごとの距離を出そうとしているのですが上手くいきません．

コードは以下のように書いています．xb,xcはそれぞれ常微分方程式を解いて得られたr，r'，θ，θ'がこの順に列となって格納されています．2642秒まで1秒ごとにその点のr, θが求められています．

どうすれば良いでしょうか，ご教授お願い致します．

distances = zeros(2642,1);
for cnt=1:2642
    x1(cnt) = xb(cnt,1)*cos(xb(cnt,3));
    y1(cnt) = xb(cnt,1)*sin(xb(cnt,3));
    x2(cnt) = xc(cnt,1)*cos(xc(cnt,3));
    y2(cnt) = xc(cnt,1)*sin(xc(cnt,3));
   distance(cnt)= sqrt((x1(cnt)-x2(cnt))^2+(y1(cnt)-y2(cnt))^2);
end;

[Takumi]

上記のコードでほぼ問題ないように見えますが，どのようにうまく求まらないのでしょうか．

また，次のようにアニメーションにすると何がうまくいかないか確認しやすいかもしれません．

clear
close all

load xb.mat
load xc.mat
% r, r', theta, theta'

% 極座標
figure;
polarplot(xb(:,3),xb(:,1),'.');
hold on
polarplot(xc(:,3),xc(:,1),'.');

% デカルト座標
x1 = xb(:,1).*cos(xb(:,3));
y1 = xb(:,1).*sin(xb(:,3));
x2 = xc(:,1).*cos(xc(:,3));
y2 = xc(:,1).*sin(xc(:,3));

N = numel(x2);
figure;
plot(x1,y1,'-r',x2,y2,'-b');hold on;axis equal;
h1 = plot(x1(1),y1(1),'.r');
h2 = plot(x2(1),y2(1),'ob');
for i=2:10:N
    h1.XData = x1(i);
    h1.YData = y1(i);
    h2.XData = x2(i);
    h2.YData = y2(i);
    drawnow
    pause(0.1);
end
% 距離
distance= sqrt((x1(1:N)-x2).^2+(y1(1:N)-y2).^2);
figure;
plot(distance);
xlabel('time step');
ylabel('distance');