intersect_old.m

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

function CON = intersect_old(CONfeas,CONadd,clean_up_flag)

% Compute the intersection of two linear constraint sets
%
% Syntax:
%   "C = intersect(a,b,clean_up_flag)"
%
% Description:
%   "intersect(a,b,clean_up_flag)" returns a linearcon object containing
%   the intersection of "a" and "b".  If no intersection exists,
%   "intersect(a,b,clean_up_flag)" returns an empty linear constraint
%   object.  The value of "clean_up_flag" determines whether or not
%   redundant constraints are removed from the returned linear constraint
%   object;  "clean_up_flag=1"--remove all redundant constraints,
%   "clean_up_flag=0"--do nothing.
%
% Note:
%   At most one equality constraint is allowed in "a", and if "b" has an
%   equality constraint, it must be the same as the one present in "a".
%
% See Also:
%   linearcon,isfeasible,and

global GLOBAL_APPROX_PARAM
global GLOBAL_OPTIM_PAR

epsilon = GLOBAL_APPROX_PARAM.poly_epsilon;
hyperplane_tol = GLOBAL_APPROX_PARAM.poly_hyperplane_tol;
CON = linearcon;

if isempty(CONfeas) | isempty(CONadd)
  return
end

if (length(CONfeas.dE) > 1) | (length(CONadd.dE) > 1)
  fprintf('\007intersect: Invalid constraints given, more than 1 equality found\n')
  return
end

if (length(CONfeas.dE) == 0) & (length(CONadd.dE) == 1)
  fprintf('\007intersect: Invalid constraints given, additional equality constraints\n')
  return
end

% If an equality constraint is found in both sets of constraints,
% check if they're the same constraint, if not return an empty
% cell array

if (length(CONfeas.dE) == 1) & (length(CONadd.dE) == 1)
  MATRIX = [CONfeas.CE CONfeas.dE
            CONadd.CE  CONadd.dE];
  if rank(MATRIX,hyperplane_tol) > 1
%    fprintf('intersect: Patches w/ different eq constraints found\n')
    return
  else
    CONadd.CE = []; CONadd.dE = [];
  end
end  

% Start with the feasible constraints
CE = CONfeas.CE; dE = CONfeas.dE;
CI = CONfeas.CI; dI = CONfeas.dI;

% Find out if each new inequality constraint is feasible
CIadd = CONadd.CI; dIadd = CONadd.dI;
for k = 1:size(CIadd,1)
  cIk = CIadd(k,:);
  dIk = dIadd(k);
  if is_new_ineq(CONfeas,cIk,dIk)
    xmin = linprog(cIk',CI,dI,CE,dE,[],[],[],GLOBAL_OPTIM_PAR);
 %  xmin = lp(cIk',[CE; CI],[dE; dI],[],[],[],size(CE,1));
    fmin = cIk*xmin;
    isfeasible = (fmin < dIk-epsilon);
    if isfeasible
      CI = [CI; cIk];
      dI = [dI; dIk];
    else
      return
    end
  end
end

CON = linearcon(CE,dE,CI,dI);
if clean_up_flag
  CON = clean_up(CON);
end

return


% ----------------------------------------------------------------------------
function new = is_new_ineq(CONfeas,c,d)

% Check if the given constraint cTx <= d
% is already in the feasible constraint CONfeas

global GLOBAL_APPROX_PARAM

hyperplane_tol = GLOBAL_APPROX_PARAM.poly_hyperplane_tol;

% First search the equality constraints
CE = CONfeas.CE; dE = CONfeas.dE;
new = 1;
for k = 1:length(dE)
  MATRIX = [CE(k,:) dE(k); c d];
  if (rank(MATRIX,hyperplane_tol) < 2)
    new = 0;
    break;
  end
end

if new
  % Now search the inequality constraints
  CI = CONfeas.CI; dI = CONfeas.dI;
  for k = 1:length(dI)
    MATRIX = [CI(k,:) dI(k); c d];
    if (rank(MATRIX,hyperplane_tol) < 2) & (CI(k,:)*c' > 0)
      new = 0;
      break;
    end
  end
end
return