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 |
---|