[37] | 1 | function [Q,c,f,x,info] = quaddecomp(p,z) |
---|
| 2 | %QUADDECOMP Internal function to decompose quadratic expression |
---|
| 3 | |
---|
| 4 | % Author Johan Löfberg |
---|
| 5 | % $Id: quaddecomp.m,v 1.11 2006/09/14 13:02:51 joloef Exp $ |
---|
| 6 | |
---|
| 7 | [n,m]=size(p); |
---|
| 8 | info = 0; |
---|
| 9 | |
---|
| 10 | % Is it a scalar |
---|
| 11 | if (n*m==1) |
---|
| 12 | % Involved in polynomial expression |
---|
| 13 | [mt,variabletype] = yalmip('monomtable'); |
---|
| 14 | x_lin = getvariables(p); |
---|
| 15 | x_var = find(any(mt(x_lin,:),1)); |
---|
| 16 | if nargin==2 |
---|
| 17 | x_var = union(x_var,depends(z)); |
---|
| 18 | end |
---|
| 19 | x = recover(x_var); |
---|
| 20 | if all(variabletype(x_lin) ==0)% is(p,'linear') |
---|
| 21 | n = length(x); |
---|
| 22 | Q = spalloc(n,n,0); |
---|
| 23 | fc = getbase(p); |
---|
| 24 | f = fc(1); |
---|
| 25 | if nargin==2 |
---|
| 26 | vars = getvariables(p); |
---|
| 27 | c = zeros(length(x),1); |
---|
| 28 | for i = 1:length(vars) |
---|
| 29 | c(find(vars(i)==x_var)) = fc(1+i); |
---|
| 30 | end |
---|
| 31 | else |
---|
| 32 | c = fc(2:end);c=c(:); |
---|
| 33 | end |
---|
| 34 | return |
---|
| 35 | end |
---|
| 36 | % degrees = sum(mt(x_lin,:),2); |
---|
| 37 | variabletype = variabletype(x_lin); |
---|
| 38 | if all(variabletype<=2)%all(degrees<=2) |
---|
| 39 | %Q = spalloc(length(x),length(x),2*nnz(getbase(p))); |
---|
| 40 | %c = zeros(length(x),1); |
---|
| 41 | base = getbase(p); |
---|
| 42 | if nnz(base(1))==0 |
---|
| 43 | f = 0; |
---|
| 44 | base = base(2:end); |
---|
| 45 | else |
---|
| 46 | f = base(1); |
---|
| 47 | base = base(2:end); |
---|
| 48 | end |
---|
| 49 | |
---|
| 50 | mt = mt(x_lin,x_var); |
---|
| 51 | |
---|
| 52 | if 1 |
---|
| 53 | |
---|
| 54 | quads = find (variabletype == 2);% & base); |
---|
| 55 | bilins = find (variabletype == 1);% & base); |
---|
| 56 | consts = find (variabletype == 0);% & base); |
---|
| 57 | |
---|
| 58 | [varsC,aux1,aux2] = find(mt(consts,:)'); |
---|
| 59 | [varsQ,aux1,aux2] = find(mt(quads,:)'); |
---|
| 60 | [varsB,aux1,aux2] = find(mt(bilins,:)'); |
---|
| 61 | |
---|
| 62 | if isempty(varsQ) |
---|
| 63 | varsQ = []; |
---|
| 64 | end |
---|
| 65 | if isempty(varsB) |
---|
| 66 | varsB = []; |
---|
| 67 | end |
---|
| 68 | if isempty(varsC) |
---|
| 69 | varsC = []; |
---|
| 70 | end |
---|
| 71 | |
---|
| 72 | c = sparse(varsC,1,base(consts),length(x),1); |
---|
| 73 | %c(varsC) = base(consts);c = c(:); |
---|
| 74 | %c1 = c; |
---|
| 75 | ii = [varsQ ; varsB(1:2:end) ; varsB(2:2:end)]; |
---|
| 76 | jj = [varsQ ; varsB(2:2:end) ; varsB(1:2:end)]; |
---|
| 77 | kk = [base(quads) base(bilins)/2 base(bilins)/2]; |
---|
| 78 | Q = sparse(ii,jj,kk,length(x),length(x)); |
---|
| 79 | %Q1 = sparse(varsQ,varsQ,base(quads),length(x),length(x)); |
---|
| 80 | %Q2 = sparse(varsB(1:2:end),varsB(2:2:end),base(bilins)/2,length(x),length(x)); |
---|
| 81 | %Q3 = sparse(varsB(2:2:end),varsB(1:2:end),base(bilins)/2,length(x),length(x)); |
---|
| 82 | %Q = Q1 + (Q2+Q3); |
---|
| 83 | |
---|
| 84 | else |
---|
| 85 | Q = spalloc(length(x),length(x),2*nnz(getbase(p))); |
---|
| 86 | c = zeros(length(x),1); |
---|
| 87 | [jj,ii,kk] = find(mt');ii = [ii(:) ;0]; |
---|
| 88 | top = 1; |
---|
| 89 | for i = 1:length(x_lin) |
---|
| 90 | if ii(top) == ii(top+1) |
---|
| 91 | j = [jj(top) jj(top+1)]; |
---|
| 92 | top = top + 2; |
---|
| 93 | else |
---|
| 94 | j = [jj(top)]; |
---|
| 95 | top = top + 1; |
---|
| 96 | end |
---|
| 97 | if length(j)==1 % One variable |
---|
| 98 | if kk(top-1)==1 |
---|
| 99 | c(j)=base(i); |
---|
| 100 | else |
---|
| 101 | Q(j,j)=Q(j,j) + base(i)/2; |
---|
| 102 | end |
---|
| 103 | else |
---|
| 104 | Q(j(1),j(2))=Q(j(1),j(2)) + base(i)/2; |
---|
| 105 | end |
---|
| 106 | end |
---|
| 107 | Q = Q+Q'; |
---|
| 108 | % |
---|
| 109 | % norm(Q-Q1,'inf') |
---|
| 110 | % norm(c-c1,'inf') |
---|
| 111 | end |
---|
| 112 | |
---|
| 113 | else |
---|
| 114 | if nargout==5 |
---|
| 115 | info = 1; |
---|
| 116 | Q = []; |
---|
| 117 | c = []; |
---|
| 118 | f = []; |
---|
| 119 | x = []; |
---|
| 120 | else |
---|
| 121 | error('Function is not quadratic'); |
---|
| 122 | end |
---|
| 123 | end |
---|
| 124 | |
---|
| 125 | else |
---|
| 126 | if nargout==5 |
---|
| 127 | info = 1; |
---|
| 128 | Q = []; |
---|
| 129 | c = []; |
---|
| 130 | f = []; |
---|
| 131 | x = []; |
---|
| 132 | else |
---|
| 133 | error('Function is not scalar'); |
---|
| 134 | end |
---|
| 135 | end |
---|