function [ A ] = constraint_matrix( S )
%CONSTRAINT_MATRIX Generate constraint matrix A for Aq = 0
%   S = m*2 target shape. Each row represents (x,y) point
%   A = 2(m-2)*2m matrix
%   q must be 2m long vector with odd elements x and even elements y
%   S must have s(1,:) = (0, 0) and s(2,2) = 0

sx = S(:,1);
sy = S(:,2);
m = length(S);

s2 = norm(S(2,:), 2);
A = zeros(2*(m-2), 2*m);
for k = 1:m-2
    i = 2+k; % i = 3:m
    row = 2*k - 1;
    col = 2*i - 1;
   % First constraint
   A(0 + row, 0 + col) = s2;
   A(0 + row, 0 + 1) = -s2;

   A(0 + row, 1) = A(0 + row, 1) + sx(i);
   A(0 + row, 3) = -sx(i);
   A(0 + row, 2) = -sy(i);
   A(0 + row, 4) = sy(i);

   % Second constraint
   A(1 + row, 1 + col) = s2;
   A(1 + row, 1 + 1) = -s2;
   A(1 + row, 1) = sy(i);
   A(1 + row, 3) = -sy(i);
   A(1 + row, 2) = A(1 + row, 2) + sx(i);
   A(1 + row, 4) = -sx(i);
end

end

