DIRECTORY
QPSolve, RealFns
;

QPSolveImpl: CEDAR PROGRAM
IMPORTS RealFns
EXPORTS QPSolve
~ BEGIN
OPEN QPSolve;

NewMatrix: PUBLIC PROC [m, n: NAT] RETURNS [M: Matrix]
~ {
M _ NEW[MatrixRep[m]];
M.m _ m;
FOR i: NAT IN [0..m) DO M[i] _ NEW[RVectorRep[n]]; M[i].n _ n ENDLOOP;
};

QPSolve: PUBLIC PROC [
c: RVector, A: Matrix, lobd: RVector, x: RVector, iVar: IVector, nFR: NAT]
RETURNS [minFR: NAT]
~ {
n: NAT ~ x.n;			-- number of variables
m: NAT ~ A.m;			-- number of general (equality) constraints
mL: NAT _ m;			-- number of active non-bounds constraints
nZ: NAT _ nFR-mL;	-- number of degrees of freedom active in x
Q: Matrix _ NewMatrix[n, n];		-- will contain ((Z Y 0)T(0 0 IFX)T)T = Q
T: Matrix _ NewMatrix[m, n];		-- will contain T of TQ decomposition of A
g: RVector _ NEW[RVectorRep[n]];	-- gradient at current point
Zg: RVector _ NEW[RVectorRep[n]];	-- projected gradient at current point
p: RVector _ NEW[RVectorRep[n]];	-- step direction
toMin: REAL _ 0.0;					-- step magnitude to projected minimum or nearest bound
lL: RVector _ NEW[RVectorRep[m]];	-- Lagrange multiplier estimates for general constraints
lFX: RVector _ NEW[RVectorRep[n]];	-- Lagrange multiplier estimates for bound constraints
b: RVector _ NEW[RVectorRep[m]];		-- temporary storage vector
GetMat: TYPE ~ PROC [i, j: NAT] RETURNS [REAL];
PutMat: TYPE ~ PROC [i, j: NAT, val: REAL];
getA: GetMat ~ {RETURN[A[i][iVar[j]]]};
getQ: GetMat ~ {RETURN[Q[iVar[i]][j]]};
putQ: PutMat ~ {Q[iVar[i]][j] _ val};
getT: GetMat ~ {RETURN[T[i][j]]};
putT: PutMat ~ {T[i][j] _ val};
fullProject: BOOL _ FALSE;
eps: REAL _ 5.0e-5;
gMag: REAL;
EstimateLagrange: PROC
~ {
FOR i: NAT IN [0..mL) DO			-- Iterate across columns of YFR generating b.
dot: REAL _ 0.0;					-- Dot column of YFR with gFR.
FOR j: NAT IN [0..nFR) DO dot _ dot + getQ[j, nZ+i]*g[j] ENDLOOP;
b[i] _ dot;
ENDLOOP;
FOR i: NAT IN [0..mL) DO			-- Iterate down rows of TT generating lL.
sum: REAL _ b[i];				-- Dot row of TT with lL.
FOR j: NAT IN [mL-i..mL) DO sum _ sum - getT[j, nZ+i]*lL[j] ENDLOOP;
lL[mL-1-i] _ sum/getT[mL-1-i, nZ+i];
ENDLOOP;
FOR i: NAT IN [nFR..n) DO
sum: REAL _ g[i];				-- Dot row of AFXT with lL.
FOR j: NAT IN [0..mL) DO sum _ sum - getA[j, i]*lL[j] ENDLOOP;
lFX[i] _ sum;
ENDLOOP;
};
NegativeLagrange: PROC RETURNS [k: NAT]
~ {
mostNeg: REAL _ -eps;
k _ n;
FOR i: NAT IN [nFR..n) DO
IF lFX[i] < mostNeg THEN {k _ i; mostNeg _ lFX[i]};
ENDLOOP;
};
UpdateGradient: PROC
~ {
gMag _ 0.0;
FOR i: NAT IN [0..n) DO gMag _ MAX[gMag, ABS[g[i] _ x[iVar[i]] + c[iVar[i]]]] ENDLOOP
};
ProjectGradient: PROC
~ {
FOR i: NAT IN [0..nZ) DO
dot: REAL _ 0.0;
FOR j: NAT IN [0..nFR) DO dot _ dot + getQ[j, i]*g[j] ENDLOOP;
Zg[i] _ -dot;
ENDLOOP;
};
NegligibleZGradient: PROC RETURNS [BOOL]
~ {
toMin _ 0.0;
IF fullProject
THEN {
ProjectGradient[];
FOR i: NAT IN [0..nZ) DO IF ABS[Zg[i]] > eps THEN toMin _ 1.0 ENDLOOP;
}
ELSE {FOR j: NAT IN [0..nFR) DO toMin _ toMin+g[j]*getQ[j, nFR-1] ENDLOOP};
RETURN[ABS[toMin] < eps];
};
ComputeDirection: PROC
~ {
IF fullProject
THEN {
FOR i: NAT IN [0..nFR) DO p[i] _ 0.0 ENDLOOP;
FOR i: NAT IN [0..nZ) DO
FOR j: NAT IN [0..nFR) DO p[j] _ p[j] + getQ[j, i]*Zg[i] ENDLOOP;
ENDLOOP;
}
ELSE {
IF toMin > 0.0
THEN {FOR j: NAT IN [0..nFR) DO p[j] _ getQ[j, nFR-1] ENDLOOP}
ELSE {FOR j: NAT IN [0..nFR) DO p[j] _ -getQ[j, nFR-1] ENDLOOP};
toMin _ ABS[toMin];
};
};
NearestBound: PROC RETURNS [k: NAT]
~ {
toK: REAL;
fudge: REAL _ eps;
k _ nFR;
FOR i: NAT IN [0..nFR) DO
IF p[i] < -fudge AND (toK _ (lobd[iVar[i]]-x[iVar[i]])/p[i]) < toMin AND toK > 0.0
THEN {toMin _ toK; k _ i};
ENDLOOP;
};
StepToMin: PROC
~ {FOR i: NAT IN [0..nFR) DO x[iVar[i]] _ x[iVar[i]] + toMin*p[i] ENDLOOP};
MakeRot: PROC [x, y: REAL] RETURNS [c, s: REAL]
~ {
Signum: PROC [v: REAL] RETURNS [REAL]
~ INLINE {RETURN[IF v < 0.0 THEN -1.0 ELSE 1.0]};
IF x = 0.0
THEN {c _ IF y < 0.0 THEN -1.0 ELSE 1.0;  s _ 0.0}
ELSE {
IF ABS[y] > ABS[x]
THEN {t: REAL _ x/y;  c _ Signum[y]/RealFns.SqRt[1+t*t];  s _ c*t}
ELSE {t: REAL _ y/x;  s _ Signum[x]/RealFns.SqRt[1+t*t];  c _ s*t};
};
};
RotCols: PROC [c, s: REAL, getM: GetMat, putM: PutMat, loRow, hiRow, xCol, yCol: NAT]
~ {
IF c = 1.0 THEN RETURN;
FOR i: NAT IN [loRow..hiRow) DO
x: REAL _ getM[i, xCol];
y: REAL _ getM[i, yCol];
xRot: REAL _ x*c - y*s;
yRot: REAL _ x*s + y*c;
putM[i, xCol, xRot];
putM[i, yCol, yRot];
ENDLOOP;
};
FixVar: PROC [k: NAT]
~ {
kV: NAT _ iVar[k];
FOR i: NAT IN [k..nFR-1) DO iVar[i] _ iVar[i+1] ENDLOOP;
iVar[nFR-1] _ kV;
nFR _ nFR-1;
nZ _ nZ-1;
FOR i: NAT IN [0..nFR) DO
x: REAL _ getQ[nFR, i];
y: REAL _ getQ[nFR, i+1];
c, s: REAL;
[c, s] _ MakeRot[x, y];
RotCols[c, s, getQ, putQ, 0, nFR+1, i, i+1];
SELECT i FROM
= nZ => {
FOR j: NAT IN [0..mL) DO putT[j, i, 0.0] ENDLOOP;
RotCols[c, s, getT, putT, mL-1, mL, i, i+1];
};
> nZ => RotCols[c, s, getT, putT, nFR-1-i, mL, i, i+1];
ENDCASE;
ENDLOOP;
};
FreeVar: PROC [k: NAT]
~ {
kV: NAT _ iVar[k];
FOR i: NAT DECREASING IN [nFR..k) DO iVar[i+1] _ iVar[i] ENDLOOP;
iVar[nFR] _ kV;
nFR _ nFR+1;
nZ _ nZ+1;
FOR j: NAT IN [0..nFR-1) DO putQ[j, nFR-1, 0.0]; putQ[nFR-1, j, 0.0] ENDLOOP;
putQ[nFR-1, nFR-1, 1.0];
FOR j: NAT IN [0..mL) DO putT[j, nFR-1, getA[j, nFR-1]] ENDLOOP;
FOR i: NAT DECREASING IN [nZ..nFR) DO
x: REAL _ getT[nFR-1-i, i-1];
y: REAL _ getT[nFR-1-i, i];
c, s: REAL;
[c, s] _ MakeRot[x, y];
RotCols[c, s, getQ, putQ, 0, nFR, i-1, i];
RotCols[c, s, getT, putT, nFR-1-i, mL, i-1, i];
ENDLOOP;
};
InitTQ: PROC
~ {
FOR i: NAT IN [0..nFR) DO
FOR j: NAT IN [0..mL) DO putT[j, i, getA[j, i]] ENDLOOP;
FOR j: NAT IN [0..nFR) DO putQ[j, i, IF i=j THEN 1.0 ELSE 0.0] ENDLOOP;
ENDLOOP;
FOR j: NAT IN [0..mL) DO
FOR i: NAT IN [1..nFR-j) DO
c, s: REAL;
[c, s] _ MakeRot[getT[j, i-1], getT[j, i]];
RotCols[c, s, getT, putT, j, mL, i-1, i];
RotCols[c, s, getQ, putQ, 0, nFR, i-1, i];
ENDLOOP;
ENDLOOP;
};
iter: NAT _ 0;
fullProject _ nFR > 1;
InitTQ[];
FOR iter IN [1..5*n*n] DO
k: NAT;
UpdateGradient[];
IF NegligibleZGradient[]
THEN {
EstimateLagrange[];
IF (k _ NegativeLagrange[]) < n THEN FreeVar[k] ELSE EXIT;
}
ELSE {
ComputeDirection[];
k _ NearestBound[];
StepToMin[];
IF k < nFR THEN FixVar[k];
};
ENDLOOP;
RETURN[nFR];
};


END..
  ю  
QPSolveImpl.mesa
Copyright ╙ 1990 by Xerox Corporation.  All rights reserved.
Ken Shoemake, April 17, 1990 11:43 pm PDT


Procedures
Solve simple Quadratic Program: minimize | xTx + cTx, subject to Ax = 0 and x > lobd.
Iterations start with initial feasible vector x, with x[i B iFX] fixed on their bounds and the rest, x[i B iFR], free.  Elements [0..nFR) of iVar store iFR, and [nFR..n) store iFX.
Method is to maintain an active set of constaints, which includes the equality constraints and some of the bounds.  The TQ factors of the currently active columns of the matrix A are maintained by incremental updating whenever a bound is added or deleted.  The T matrix is reverse lower triangular: Tij = 0 for i+j < m-1.  Part of Q forms a matrix Z, a basis for the null space of A, and the remaining columns, Y, form a basis for the range space of A.  Successive iterations use Z to compute a search direction, p.
If p is zero, then current x is a constrained minimum of active set.  Lagrange multipliers at x are estimated from the gradient g = x+c by solving first TTlL = YFRTgFR for lL, and then lFX = gFX-AFXTlL.  If lk > 0 for all bounds k, x is desired minimum; else delete a bound k with lk < 0, and do another iteration.
If p is non-zero, find nearest bound the proposed  step will encounter by computing q = MINi[qi], where qi = (si-xi)/pi, i B iFR.  If q < 1, add encountered bound i to active set.  Step x _ x + p MIN[1,q], and do another iteration.
The rows and columns of the data structures (vectors and matrices) are indexed indirectly through iFX and iFR.
<<These are indexed indirectly via iVar.>>
<<These are indexed directly.>>
<<I'm liable to go nuts keeping track of index indirection, so I'll use procedures to hide it.>>
Lagrange multipliers at x are estimated from g by solving first TTlL = YFRTgFR for lL, and then lFX = gFX-AFXTlL.
<<Compute b = YFRTgFR.>>
<<Solve TTlL = b for lL.>>
<<Now compute lFX = gFX-AFXTlL.>>
Set direction pZ to -ZTgFR.
Compute c and s so that (x y) ((c s)T (-s c)T)T = (0 w); w2 = x2+y2; w > 0.
Apply rotation (x y) ((c s)T (-s c)T)T to rows [loRow..hiRow) of columns x and y of M.
Put variable iVar[k] in fixed set iFX, and update TQ decomposition in Q and T.
Move variable k to end of iFR.  Decrement nFR, effectively moving k to iFX.
Apply rotations to Q to make row k of Q (0 0 ... 0 1).  Apply last mL rots to T.
< nZ => {};	-- In general algorithm, would update Cholesky R when i <= nZ.
<<In general algorithm, would now restore triangularity of Cholesky R.>>
Put variable iVar[k] in free set iFR, and update TQ decomposition in Q and T.
Move variable k to head of iFX.  Increment nFR, effectively moving k to iFR.
New row and column of Q are (0 0 ... 0 1).
Apply mL rotations to triangularize (T ak).  Rotations also affect augmented Q.
<<In general algorithm, would now augment then restore triangularity of Cholesky R.>>
Perform TQ factorization of initially active columns of A matrix.
Copy nFR columns of A to T.  Use Givens rotations, saved in Q, to triangularize it.
Start Code
 Э╩ж  ХNewlineDelimiter
Щ ЩJЩ<JЩ)ЧJЩ Ъ╧k	Ш	JШJШЧJШ Ъ╨nxЬЭЬЭШJЪЭЬШJЪЭЬШЧJЪЬЭШJЪЭЬ	ШJЩ Ъ╧x
Щ
JШ Ъ╧n	ЬЭЬЭЬЭЬЭЬаЬ	Ш6JШJЪЬЭЬШJШJЪЭЬЭЬЭЬЭЬЭЬЭЬШFJШJШ ЧЪаЬЭЬЭЬаЬ9ЭЬЭЬ	ЭЬШvJЪ	Ь)╧mЬ╧uЬвЬбЬЩUJЪЬ:бЬ.бЬJЩ┤JШJЪЬм╧dЬ╒ЩГJЪЬЪв╧gгЬ╨doвЬеЬдгЬдеЬеЬевдгЬдгЬбЬFдгЬЩ║JЪЬTдЬгЬдгЬ	дгЬгЬгЬгЬбЬ
дЬCдЬЩчJЩnJЪЬЭЬ
╧cШ&JЪЬЭЬ
ж+Ш;JЪЬЭЬж*Ш9JЪЬЭЬж+Ш=JЩ*JЪаЬж╨cuж╨cdжзжзжШGJЪаЬж*ШHJЩJЪЬЭЬжШ=JЪЬЭЬж&ШHJЪЬЭЬжШ2JЪЬЭЬж<ШNJЪдЬЭЬж9ШZJЪдЬЭЬж7ШYJЪЬЭЬжШ=Щ`JЪЬЭЬЭЬЭЬЭЬЭЬШ/JЪ	ЬЭЬЭЬЭЬЭЬШ+JЪЬЭЬШ'JЪЬЭЬШ'JЪЬ%Ш%JЪЬЭЬШ!JЪЬШЧJЪЬЭЬЭЬШJЪЬЭЬ
ШJЪЬЭЬШЪаЬЭШJЪЬAвдгЬевЬеЬдгЬдеЬеЬевдгЬЩqJШJЪЬевЬеЬЩЪЭЬЭЬЭЬ	ЭЬжижШIJЪЬЭЬжижиЬШ3JЪ
ЭЬЭЬЭЬ
ЭЬ ЭЬШAJЪЬ
дШJЪЭЬШЧJЪЬ	вдгЬдгЬЩЪЭЬЭЬЭЬ	ЭЬжзж╨cgижШDJЪ	ЬЭЬжзжйиЬШ.JЪЭЬЭЬЭЬЭЬдЬЭЬШDJЪдЬ#Ш$JЪЭЬШЧJЪЬдеЬеЬевдгЬЩ!ЪЭЬЭЬЭЬ
ЭШJЪ
ЬЭЬж╤cdoзжйижШ0JЪЭЬЭЬЭЬ	ЭЬдЬЭЬШ>JЪдЬШJЪЭЬШЧJШЧЪаЬЭЬЭЬЭЬШ'JЪЬШJЪЬ	ЭЬШJШЪЭЬЭЬЭЬ
ЭШJЪЭЬдЬЭЬдЬШ3JЪЭЬШЧJЪЬШЧЪаЬЭШJЪЬШJШJЪЭЬЭЬЭЬЭЬЭЬЭЬ"ЭШUJЪЬШЧЪаЬЭШJЪЬгЬвЬгЬЩJЪЬШЪЭЬЭЬЭЬ	ЭШJЪЬЭЬШJЪ
ЭЬЭЬЭЬ
ЭЬЭЬШ>JШJЪЭЬШЧJЪЬШЧЪаЬЭЬЭЬЭЬШ(JЪЬШJШЪЭЬШЪЭЬШJШJЪЭЬЭЬЭЬ	ЭЬЭЬЭЬЭЬЭЬШFJШЧJЪЭЬЭЬЭЬЭЬ
ЭЬ#ЭЬШKЧJЪЭЬЭЬШJЪЬШЧЪаЬЭШJЪЬШЪЭЬШЪЭЬШJЪ
ЭЬЭЬЭЬ
ЭЬЭЬШ-ЪЭЬЭЬЭЬ	ЭШJЪ
ЭЬЭЬЭЬ
ЭЬ ЭЬШAJЪЭЬШЧJШЧЪЭЬШЪЭЬШJЪЭЬЭЬЭЬЭЬ
ЭЬЭЬШ>JЪЭЬЭЬЭЬЭЬ
ЭЬЭЬШ@ЧJЪЬЭЬШJШЧЧJШЧЪаЬЭЬЭЬЭЬШ#JЪЬШJЪЬЭЬШ
JЪЬЭЬШJШЪЭЬЭЬЭЬ
ЭШЪЭЬЭЬ1ЭЬ
ШRJЪЭЬШЧJЪЭЬШЧJШЧЪа	ЬЭШJЪЬЭЬЭЬЭЬ
ЭЬ&ЭЬШKЧЪ
аЬЭЬЭЬЭЬЭЬШ/JЪЬ$вЬвЬвЬвЬвЬвЬбЬЩKJШЪ
аЬЭЬЭЬЭЬЭЬШ%JЪЬЭЬЭЬЭЬ	ЭЬЭЬШ1ЧЪЭЬШ
JЪЭЬЭЬ	ЭЬЭЬШ2ЪЭЬШЪЭЬЭЬЭЬШJЪЭЬЭЬ5ШBJЪЭЬЭЬ6ШCЧJШЧЧJШЧЪаЬЭЬЭЬ8ЭЬШUJЪЬвЬвЬвЬ0ЩVJШJЪЭЬ	ЭЬЭЬШЪЭЬЭЬЭЬЭШJЪЬЭЬШJЪЬЭЬШJЪЬЭЬШJЪЬЭЬШJЪЬШJЪЬШJЪЭЬШЧJШЧЪаЬЭЬЭЬШJЩNJШJЩKJЪЬЭЬШJЪ
ЭЬЭЬЭЬЭЬЭЬШ8JШJШJШ
JЪЬDгЬЩPЪЭЬЭЬЭЬ
ЭШJЪЬЭЬШJЪЬЭЬШJЪЬЭЬШJШJШ,ЪЭЬЭШJЪЬж=ЬЩJЪЬ	Ш	JЪ
ЭЬЭЬЭЬ	ЭЬЭЬШ1JЪЬ,Ш,JЪЬШЧJЪЬ7Ш7JЪЭЬШЧJЪЭЬШЧJЪЬHЩHJШЧЪаЬЭЬЭЬШJЩMJШJЩLJЪЬЭЬШJЪЭЬЭЬЭ
ЬЭЬ
ЭЬЭЬШAJШJШJШ
JЪЬ*Щ*JЪ
ЭЬЭЬЭЬЭЬ*ЭЬШMJЪЬШJЪЬгЬ гЬ&ЩOJЪ
ЭЬЭЬЭЬ	ЭЬ ЭЬШ@Ъ	ЭЬЭЬЭ
ЬЭЬЭШ%JЪЬЭЬШJЪЬЭЬШJЪЬЭЬШJШJШ*JЪЬ/Ш/JЪЭЬШЧJЪЬUЩUJШЧЪаЬЭШJЩAJШJЩSЪЭЬЭЬЭЬ
ЭШJЪ
ЭЬЭЬЭЬ	ЭЬЭЬШ8JЪЭЬЭЬЭЬ
ЭЬЭЬЭЬЭЬЭЬШGJЪЭЬШЧЪЭЬЭЬЭЬ	ЭШЪЭЬЭЬЭЬЭШJЪЬЭЬШJШ+JШ)JШ*JЪЭЬШЧJЪЭЬШЧJШЧJЪЬЭЬШJЪЬШJШ	ЪЭЬЭЬЭШJЪЬЭЬШJШЪЭЬШЪЭЬШJЪЬШJЪЭЬЭЬЭЬЭЬШ:JЪЬШЧЪЭЬШJЪЬШJЪЬШJЪЬШJЪЭЬ	ЭЬШJЪЬШЧЧJЪЭЬШЧJЪЭЬШJШJШ ЧЧЪЯ
Щ
JШ ЧJЪЭЬШЧ ЕЧ    р  0t  