convex_hull.m

CheckMate is a MATLAB-based tool for modeling, simulating and investigating properties of hybrid dyn

function [CH,CI,dI] = convex_hull(P)

% Converts a vertices object into a polyhedron object.
%
% Syntax:
%   "[CH,CI,dI] = convex_hull(P)"
%
% Description:
%   "convex_hull(P)" returns a polyhedron object CH constructed as the
%   convex hull of the points in the vertices object "P", and the matrices
%   "CI" and "dI" that determine the inequalities of "CH".
%   Matlab's QHULL routine, which is based on a Quickhull search algorithm,
%   is used to compute the convex hull.
%
%   The following rules determine the construction of the polyhedron object:
%   
%   - If the vertex object contains just one point, the polyhedron object
%     is n-dimensional with the length 2*vector_tol in all dimensions.
%
%   - If the vertices determine a full-dimensional hull the polyhedron
%     object is of course full dimensional.
%
%   - If the vertices determine an (n-1)-dimensional hull the polyhedron
%     object is not bloated, i.e., it is (n-1)-dimensional.
%
%   - If the convex hull of the vertices is of a dimension lower than (n-1)
%     but larger than zero, the polyhedron is bloated to full dimension,
%     i.e., this routine does never return an object that is of dimension
%     less than n-1.
%
%   Note: Since this routine accesses fields of polyhedron objects it can
%   only be used if it is located within the method of this class, i.e.,
%   if it is in the "@polyhedron" folder of the Checkmate distribution.
%
% See Also:
%   polyhedron,vertices
%
%
% --OS-- Last change: 06/05/02
%

global GLOBAL_APPROX_PARAM

CH=polyhedron;
CI=[]; dI=[]; VI=cell(1,1);
CE=[]; dE=[]; VE=cell(1,1);
ni=0; ne=0;

roundtol = GLOBAL_APPROX_PARAM.poly_vector_tol;
if (roundtol==0)
   roundtol=1e3*eps;
end

svdtol = GLOBAL_APPROX_PARAM.poly_svd_tol;
if (svdtol==0)
   svdtol=1e4*eps;
end

% Absolute value for bloating
bloattol = GLOBAL_APPROX_PARAM.poly_bloat_tol;
% must be larger than 'roundtol'!
if bloattol<=roundtol+10*eps;
   bloattol=roundtol+10*eps;
end

% Rounding and Removal of points with multiple occurrence:
verts=[];
for i=1:length(P)
   verts=[verts; roundtol*round(P(i)'/roundtol)];
end
verts=unique(verts,'rows');
n=size(verts,2);
m=size(verts,1);

%--------------------------------------------------------------------------
% CASE 1: Input contains only one point
%         The convex hull is a hyperrectangular box obtained from bloating.
%--------------------------------------------------------------------------
if (m==1)
   CI=zeros(2*n,n);
   Ver=[];
   for (i=1:n)
      CI(2*i-1,i)=-1; dI(2*i-1,1)=-verts(i)+bloattol;    
      CI(2*i,i)=1; dI(2*i,1)=verts(i)+bloattol;
      jM=[];     
      for j=1:n
         if (j~=i)
            jMa=[jM;(verts(j)-bloattol)*ones(1,max(size(jM,2),1))];
            jMb=[jM;(verts(j)+bloattol)*ones(1,max(size(jM,2),1))];
            jM=[jMa,jMb];    
        end    
      end
      jMm=[jM(1:i-1,:);(verts(i)-bloattol)*ones(1,size(jM,2));jM(i:size(jM,1),:)];
      jMp=[jM(1:i-1,:);(verts(i)+bloattol)*ones(1,size(jM,2));jM(i:size(jM,1),:)];
      Ver=[Ver,jMm,jMp];
      VI{2*i-1}=vertices(jMm); VI{2*i}=vertices(jMp);
   end   
   CH.CI=CI; CH.dI=dI; CH.VI=VI;
   CH.CE=CE; CH.dE=dE; CH.VE=VE; 
   CH.vtcs=vertices(unique(Ver','rows')');
   return 
end    
%---- END OF CASE 1 -------------------------------------------------------


% Singular Value Decomposition:
x_org=verts(1,:);
dverts=[];
for i=1:size(verts,1)
   dverts=[dverts;verts(i,:)-x_org];
end
[U,S,V]=svd(dverts);
VT=V';

% Determination of the Rank Deficiency depending on 'roundtol'
rs=0;
for i=1:min(size(S))
   if abs(S(i,i))<svdtol
      S(i,i)=0; 
   else    
      rs=rs+1;        % rs: rank of manipulated S   
   end
end
rd=n-rs;              % rd: rank deficiency

if (rd>1)
   warning([' CONVEX_HULL leads to a hull of dimension ',num2str(rs),' - the hull is expanded to full dimension!']);
end

%---- CASE 2: Points form an n-dimensional object "normal case" -----------
if (rs==n)

   % Compute the convexhull in the original space
   chi=convhulln(verts);

   eqverts=[]; Ver=[];
   for i=1:size(chi,1) % Loop over all facets
      facetverts=[];
      for j=1:n
         facetverts=[facetverts,verts(chi(i,j),:)'];
      end
      [c,d]=points2hyperplane(vertices(facetverts));
         
      % Check if 'c' points to outside, and correct it if not:
      if ~isempty(c)
         k=1;
         while (k<=m)
            if ~ismember(k,chi(i,:))
               if (c'*verts(k,:)'-d > 10*eps)
                  c=-c; d=-d;
                  break
               end
            end     
            k=k+1;   
         end    
         Ver=[Ver,facetverts];
         CI=[CI;c']; dI=[dI;d]; 
         ni=ni+1; VI{ni}=vertices(facetverts);      
      end   
   end   
   CH.CI=CI; CH.dI=dI; CH.VI=VI;
   CH.CE=CE; CH.dE=dE; CH.VE=VE; 
   CH.vtcs=vertices(unique(Ver','rows')');
   CH=clean_polyhedron(CH,n);
   return
end
%---- END OF CASE 2 -------------------------------------------------------

% Transformation Matrices:
T=VT(1:rs,:);
R=VT(rs+1:rs+rd,:);

%---- CASE 3: Points form a 1-dimensional object --------------------------
if (rs==1)  % CONVHULLN cannot be called
   tdverts=(T*dverts')';
   tdmin=min(tdverts); tdmax=max(tdverts);
   CI=[VT;-VT];
   dI=[tdmax+T*x_org'+2*eps; bloattol*ones(rd,1)+R*x_org'; -tdmin-T*x_org'-2*eps; bloattol*ones(rd,1)-R*x_org']; 
   % Bloating the minimum and maximum point to 2^rd points each:
   vbmin=[tdmin-eps]; vbmax=[tdmax+eps];
   for (j=1:rd)
      jvbmin1=[vbmin; -bloattol*ones(1,max(size(vbmin,2),1))]; jvbmin2=[vbmin; bloattol*ones(1,max(size(vbmin,2),1))];
      vbmin=[jvbmin1,jvbmin2];    
      jvbmax1=[vbmax; -bloattol*ones(1,max(size(vbmax,2),1))]; jvbmax2=[vbmax; bloattol*ones(1,max(size(vbmax,2),1))];
      vbmax=[jvbmax1,jvbmax2];       
   end
   % Backtransformation of the points:
   pmin=V*vbmin; pmax=V*vbmax;
   for i=1:size(pmin,2)
      pmin(:,i)=pmin(:,i)+x_org'; pmax(:,i)=pmax(:,i)+x_org';   
   end
   p=[pmin,pmax];
   % Assignment of vertices to faces:
   vi=cell(size(CI,1),1);
   for i=1:size(p,2)
      for j=1:size(CI,1)
         if (CI(j,:)*p(:,i)<=dI(j)+10*eps)&(CI(j,:)*p(:,i)>=dI(j)-10*eps)
            vi{j}=[vi{j},p(:,i)];   
         end
      end
   end
   for j=1:size(CI,1)
      VI{j}=vertices(vi{j});
   end      
   CH.CI=CI; CH.dI=dI; CH.VI=VI;
   CH.CE=CE; CH.dE=dE; CH.VE=VE; 
   CH.vtcs=vertices(p);
   CH=clean_polyhedron(CH,n);
   return
end
%---- END OF CASE 3 -------------------------------------------------------

%---- CASE 4: Points form an (n-1)-dimensional object (and rs>1) ----------
if rd==1
   % Transformation into the rs-dimensional space
   tdverts=(T*dverts')';
   % Compute the convexhull in the transformed space
   chi=convhulln(tdverts);

   Ver=[];
   for i=1:size(chi,1) % Loop over all facets
      facetverts=[];
      for j=1:rs
         facetverts=[facetverts,tdverts(chi(i,j),:)'];
      end
      [c,d]=points2hyperplane(vertices(facetverts));

      % Determine where outside of the facet is, and reverse 'c' if necessary:
      if ~isempty(c)
         k=1;
         while (k<=size(tdverts,1))
            if ~ismember(k,chi(i,:))
               if (c'*tdverts(k,:)'-d > 10*eps)
                  c=-c; d=-d;
                  break
               end
            end     
            k=k+1;
         end   
         % Backtransformation of the relevant facet vertices:
         bfverts=V*[facetverts;zeros(1,size(facetverts,2))];
         bverts=[];
         for j=1:size(bfverts,2)
            bverts=[bverts,bfverts(:,j)+x_org'];
         end
         % Create the polyhedron object:
         CI=[CI; c'*T]; dI=[dI; d+c'*T*x_org'+2*eps]; 
         Ver=[Ver,bverts];
         ni=ni+1; VI{ni}=vertices(bverts);
      end
   end   

   CH.CI=CI; CH.dI=dI; CH.VI=VI;
   CH.CE=VT(rs+1,:); CH.dE=VT(rs+1,:)*x_org'; CH.VE{1}=vertices(unique(Ver','rows')');      
   CH.vtcs=vertices(unique(Ver','rows')');
   CH=clean_polyhedron(CH,n);
   return
end
%---- END OF CASE 4 -------------------------------------------------------

%---- CASE 5: Points form an object of dimension 2 to n-2 -----------------
if (rs>1)&(rd>1)
   % Transformation into the rs-dimensional space
   tdverts=(T*dverts')';
   % Compute the convexhull in the transformed space
   chi=convhulln(tdverts);
   
   Ver=[];
   ifcaib=0;   % identifier that is set to one once the constraints resulting from bloating are added   
   ind=[];     % index vector that stores the indices of those rows of CI which correspond to bloated directions
   for i=1:size(chi,1) % Loop over all facets
      facetverts=[];
      for j=1:rs
         facetverts=[facetverts,tdverts(chi(i,j),:)'];
      end
      [c,d]=points2hyperplane(vertices(facetverts));

      % Determine where outside of the facet is, and reverse 'c' if necessary:
      if ~isempty(c)
         k=1;
         while (k<=size(tdverts,1))
            if ~ismember(k,chi(i,:))
               if (c'*tdverts(k,:)'-d > 10*eps)
                  c=-c; d=-d;
                  break
               end
            end     
            k=k+1;   
         end    
         % Backtransformation of the relevant facet vertices:
         bfiverts=facetverts;
         for (j=1:rd)
            sbfi=size(bfiverts,2);    
            bfiverts=[[bfiverts;bloattol*ones(1,sbfi)],[bfiverts;-bloattol*ones(1,sbfi)]]; 
         end
         bfverts=V*bfiverts;   

         bverts=[];
         for j=1:size(bfverts,2)
            bverts=[bverts,bfverts(:,j)+x_org'];
         end
         % Create the polyhedron object:
         CI=[CI; c'*T]; dI=[dI; d + c'*T*x_org' + 2*eps]; 
         ni=ni+1; VI{ni}=vertices(bverts);
         Ver=[Ver,bverts];
         if (ifcaib==0)
            for (j=1:rd)
               CI=[CI; R(j,:); -R(j,:)]; dI=[dI; bloattol + R(j,:)*x_org'; bloattol - R(j,:)*x_org']; 
               ni=ni+2; ind=[ind,ni-1,ni]; VI{ni-1}=[]; VI{ni}=[];
               % The vertices are already stored in 'Ver'.
            end   
            ifcaib=1;   
         end
      end   
   end
   
   % Assign vertices:
   Ver=unique(Ver','rows')';
   for i=1:size(Ver,2)
      for j=1:length(ind)
         if (CI(ind(j),:)*Ver(:,i)<=dI(ind(j))+10*eps) & (CI(ind(j),:)*Ver(:,i)>=dI(ind(j))-10*eps)   
            vij=VI{ind(j)};
            vijk=[];
            for k=1:length(vij)
               vijk=[vijk,vij(k)];
            end
            VI{ind(j)}=vertices([vijk,Ver(:,i)]);
         end
      end
   end   
      
   CH.CI=CI; CH.dI=dI; CH.VI=VI;
   CH.CE=CE; CH.dE=dE; CH.VE=VE; 
   CH.vtcs=vertices(Ver);
   CH=clean_polyhedron(CH,n);
   return
end
%---- END OF CASE 5 -------------------------------------------------------

return  


% ----------------------------------------------------------------------
function cP = clean_polyhedron(P,n)

% This function tests whether the polyhedron objects contains inequality
% constraints that are redundant (within a tolerance vector_tol);
%
% (Equality constraints are changed. The fields 'VE' and 'vtcs' are also not updated
%  - this is impossible since it is not known which points are superfluous.)
%
% Remark: The fact that redundant inequality constraints and superfluous points
% can be contained in P is a result from the qhull-routine which uses perturbation
% of points combined with triangulation.
%

global GLOBAL_APPROX_PARAM

cP=polyhedron;

%tol=1e-8;
tol=0.9*GLOBAL_APPROX_PARAM.poly_vector_tol;

p=0; ind=[];
for i=1:size(P.CI,1)
    Cdi=[P.CI(i,:),P.dI(i,:)];
    b1=0;
    if (i>1)
       for j=1:size(cP.CI,1)
          Cdj=[cP.CI(j,:),cP.dI(j,:)];
          k=1; b2=0;
          while (k<=(size(P.CI,2)+1))
            if (abs(Cdj(k)-Cdi(k))>tol)
               b2=1;   % at least one entry different
               break
            end
            k=k+1;
          end
          if b2==0     % all components are the same
             b1=j;
             break    
          end
       end
    end
    if b1>0  % remove component -> do not copy to cP
       va=[];
       for q=1:length(cP.VI{b1})
          va=[va,cP.VI{b1}(q)];
       end    
       for q=1:length(P.VI{i})
          va=[va,P.VI{i}(q)];
       end
       va=unique(va','rows')';
       cP.VI{b1}=vertices(va);
       ind=[ind,b1];   
    else     % copy components from P tp cP
       p=p+1;
       cP.CI(p,:)=P.CI(i,:); cP.dI(p,:)=P.dI(i,:); cP.VI{p}=P.VI{i};
    end
end    

% Check the validity of vertices of eliminated faces
if (length(P.CE)==0)
   dc=n;
else
   dc=n-1;
end   
for i=1:length(ind)
   Va=[];
   for j=1:length(cP.VI{ind(i)})
      dV=cP.CI*cP.VI{ind(i)}(j)-cP.dI;
      ffc=0;
      for k=1:length(dV)
         if abs(dV(k))<=10*eps
            ffc=ffc+1;
         end
      end
      if ffc>=dc
         Va=[Va,cP.VI{ind(i)}(j)];   
      end   
   end
   cP.VI{ind(i)}=vertices(Va);
end
Ver=[];
for i=1:size(cP.CI,1)
   for j=1:length(cP.VI{i})
      Ver=[Ver,cP.VI{i}(j)];           
   end   
end
cP.vtcs=vertices(unique(Ver','rows')');
cP.VE{1}=cP.vtcs;

% No change for:
cP.CE=P.CE; cP.dE=P.dE;

return


% ----------------------------------------------------------------------
function [c,d] = points2hyperplane(P)

% Given a set of n points, calculate the normal vector 'c' and the constant 'd'
% that define the hyperplane which all points in P belong to.

global GLOBAL_APPROX_PARAM

if length(P) == 0
  c = []; d = [];
  return
end

n = dim(P);
if (length(P) ~= n)
  c = []; d = [];
  fprintf(1,'points2hyperplane: improper number of points given.\n')
  return
end

DIFF = [];
for k = 2:n
  DIFF = [DIFF (P(k)-P(1))];
end
tol = GLOBAL_APPROX_PARAM.poly_vector_tol;
if (rank(DIFF,tol) < (n-1))
  c = []; d = [];
  %fprintf(1,'points2hyperplane: given points do not form a hyperplane.\n')
  return
end

for k1 = 1:n
  additional =  [zeros(1,k1-1) max(max(abs(DIFF))) zeros(1,n-k1)]';
  T = [DIFF additional];
  if (rank(T,tol) == n)
    break;
  end
end
c = T'\[zeros(1,n-1) 1]';
c = c/sqrt(c'*c);
d = c'*average(P);
return