[Q,c]=h2c(E)

function [Q,c]=h2c(E)

% [Q,c]=h2c(E); Transforms a quadratic surface from homogeneous to center form.
% Given an nD quadratic hyper-surface in homogeneous form [x' 1]*E*[x;1]=0, 
% this function finds Q and c for the center form (x'-c')*inv(Q)*(x-c)=1
% which is used, for example, by the ellipsoid toolbox. Note that the
% symmetry on E is enforced at various points while calculating Q and c.
% Also note that the homogeneous representation E is invariant to 
% multiplication by any scalar k.
% example: 
% [Q,c]=h2c(diag([1 0.25 -1])); % zero-centered ellipse of semiaxes 1 and 2

% Conceived by Giampiero Campa & Marco Mammarella, June 2008, written by
% Marco thereafter, revised, extended and explained by Giampy, Aug. 2009,
% (while waiting for in the SFO airport for the 9:20 PM flight to LAX).
% Copyright by The MathWorks, Inc.

% argument check
if nargin~=1,
    error('h2c needs the input matrix E: [Q,c]=h2c(E);');
end

% usual preliminary checks
if isempty(E) || ~isnumeric(E) || ~isa(E,'double') || ~isreal(E),
    error('E must be a real matrix');
end

% E must be square
if size(E,1)~=size(E,2),
    error('E must be square');
end

n=size(E,1)-1;

% must be at least 2 by 2
if n<1, error('E must be at least 2 by 2'); end

% enforce symmetry
E=(E'+E)/2;

% decomposition
F=E(1:n,1:n);
h=E(1:n,n+1);

% find center
c=-F\h;

% prepare translation matrix
T=eye(n+1);
T(1:n,n+1)=c;

% translate ellipsoid into the origin
H=T'*E*T;

% find Q
k=-H(n+1,n+1);
Q=inv(H(1:n,1:n)/k);

% enforce symmetry
Q=(Q'+Q)/2;