�x�N�g���W���̃}�b�`���O�̗�

function Metrics = calc_metrics(B, p)
# metrics between all data points in 'B' and 'p'.
  Diff = (B - repmat( p, size(B)(1), 1 ))';
  # Euclidian distance
  Metrics = sum(Diff .* Diff);
  clear Diff
endfunction

function [s, is] = find_nearest_point(A, B)
  # two data point sets A and B are given
  # 2�‚̓_�W�� A, B ���^������
  num_points_A = size(A)(1);
  num_points_B = size(B)(1);

  # 'D' is a matrix to store the distance between data points A and B
  # A �̓_�� B�̓_�Ԃ̋����̓���s�� D �Ɋi�[����
  D = zeros(num_points_A, num_points_B);
  for i = 1:num_points_B
    D(:,i) = calc_metrics( A, B(i,:) );
  endfor

  # the point number in B that is nearest from each point in A
  # A �̊e�_����ł�߂� B �̓_�̓_�ԍ��� is �ɓ���
  [s, is] = min(D');

  clear D 
endfunction

# move a point
# �_�̈ړ�
function p2 = move(dx, dy, dz, p)
  p2 = p + [dx dy dz];
endfunction

# move points A and calculate find_nearest_point(A, B)
# �_�W�� A �̈ړ�. ���̌�Cfind_nearest_point(A, B)
function d2 = metrics(dx, dy, dz, A, B)
  num_points_A = size(A)(1);

  A2 = zeros(num_points_A, 3);
  for i = 1:num_points_A
    A2(i,:) = move( dx, dy, dz, A(i,:) );
  endfor

  [s, is] = find_nearest_point(A2, B);
  d2 = sum(s);

  clear A2;
endfunction

global A B

A = [0 0 0; 0 0 0; 1 1 1; 1 1 1; 2 2 2; 2 2 2];
B = [100 100 100; 101 101 101; 102 102 102];


f = @(x) metrics(x(1), x(2), x(3), A, B);
fminunc(f, [0 0 0])