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, 1992 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
 Э╩╕  Ц"cedarcode" styleХNewlineDelimiter
Щ ЩJЪЬ
╧eЬ7ЩBJЩ)JЩ ЧcodeЪ╧k	Ш	KШKШЧKШ Ъ╨nxЬЮЬЮШKЪЮЬШKЪЮЬШЧKЪЬЮШKЪЮЬ	ШJЩ Ъ╧x
Щ
KШ Ъ╧n	ЬЮЬЮЬЮЬЮЬбЬ	Ш6KШKЪЬЮЬШKШKЪЮЬЮЬЮЬЮЬЮЬЮЬШFKШKШ ЧЪбЬЮЬЮЬбЬ9ЮЬЮЬ	ЮЬШvJЪ	Ь)╧mЬ╧uЬгЬвЬЩUJЪЬ:вЬ.вЬJЩ┤KШJЪЬм╧dЬ╒ЩГJЪЬЪг╧gдЬ╨doгЬжЬедЬежЬжЬжгедЬедЬвЬFедЬЩ║JЪЬTеЬдЬедЬ	едЬдЬдЬдЬвЬ
еЬCеЬЩчJЩnKЪЬЮЬ
╧cШ&KЪЬЮЬ
з+Ш;KЪЬЮЬз*Ш9KЪЬЮЬз+Ш=JЩ*KЪбЬз╨cuз╨cdзизизШGKЪбЬз*ШHJЩKЪЬЮЬзШ=KЪЬЮЬз&ШHKЪЬЮЬзШ2KЪЬЮЬз<ШNKЪеЬЮЬз9ШZKЪеЬЮЬз7ШYKЪЬЮЬзШ=Щ`KЪЬЮЬЮЬЮЬЮЬЮЬШ/KЪ	ЬЮЬЮЬЮЬЮЬШ+KЪЬЮЬШ'KЪЬЮЬШ'KШ%KЪЬЮЬШ!KШЧKЪЬЮЬЮЬШKЪЬЮЬ
ШKЪЬЮЬШЪбЬЮШJЪЬAгедЬжгЬжЬедЬежЬжЬжгедЬЩqKШJЪЬжгЬжЬЩЪЮЬЮЬЮЬ	ЮЬзйзШIKЪЬЮЬзйзйЬШ3KЪ
ЮЬЮЬЮЬ
ЮЬ ЮЬШAKЪЬ
еШKЪЮЬШЧJЪЬ	гедЬедЬЩЪЮЬЮЬЮЬ	ЮЬзиз╨cgйзШDKЪ	ЬЮЬзизкйЬШ.KЪЮЬЮЬЮЬЮЬеЬЮЬШDKЪеЬ#Ш$KЪЮЬШЧJЪЬежЬжЬжгедЬЩ!ЪЮЬЮЬЮЬ
ЮШKЪ
ЬЮЬз╤cdoизкйзШ0KЪЮЬЮЬЮЬ	ЮЬеЬЮЬШ>KЪеЬШKЪЮЬШЧKШЧЪбЬЮЬЮЬЮЬШ'KЪЬШKЪЬ	ЮЬШKШЪЮЬЮЬЮЬ
ЮШKЪЮЬеЬЮЬеЬШ3KЪЮЬШЧKЪЬШЧЪбЬЮШKЪЬШKШKЪЮЬЮЬЮЬЮЬЮЬЮЬ"ЮШUKЪЬШЧЪбЬЮШJЪЬдЬгЬдЬЩKЪЬШЪЮЬЮЬЮЬ	ЮШKЪЬЮЬШKЪ
ЮЬЮЬЮЬ
ЮЬЮЬШ>KШKЪЮЬШЧKЪЬШЧЪбЬЮЬЮЬЮЬШ(KЪЬШKШЪЮЬШЪЮЬШKШKЪЮЬЮЬЮЬ	ЮЬЮЬЮЬЮЬЮЬШFKШЧKЪЮЬЮЬЮЬЮЬ
ЮЬ#ЮЬШKЧKЪЮЬЮЬШKЪЬШЧЪбЬЮШKЪЬШЪЮЬШЪЮЬШKЪ
ЮЬЮЬЮЬ
ЮЬЮЬШ-ЪЮЬЮЬЮЬ	ЮШKЪ
ЮЬЮЬЮЬ
ЮЬ ЮЬШAKЪЮЬШЧKШЧЪЮЬШЪЮЬШKЪЮЬЮЬЮЬЮЬ
ЮЬЮЬШ>KЪЮЬЮЬЮЬЮЬ
ЮЬЮЬШ@ЧKЪЬЮЬШKШЧЧKШЧЪбЬЮЬЮЬЮЬШ#KЪЬШKЪЬЮЬШ
KЪЬЮЬШKШЪЮЬЮЬЮЬ
ЮШЪЮЬЮЬ1ЮЬ
ШRKЪЮЬШЧKЪЮЬШЧKШЧЪб	ЬЮШKЪЬЮЬЮЬЮЬ
ЮЬ&ЮЬШKЧЪ
бЬЮЬЮЬЮЬЮЬШ/JЪЬ$гЬгЬгЬгЬгЬгЬвЬЩKKШЪ
бЬЮЬЮЬЮЬЮЬШ%KЪЬЮЬЮЬЮЬ	ЮЬЮЬШ1ЧЪЮЬШ
KЪЮЬЮЬ	ЮЬЮЬШ2ЪЮЬШЪЮЬЮЬЮЬШKЪЮЬЮЬ5ШBKЪЮЬЮЬ6ШCЧKШЧЧKШЧЪбЬЮЬЮЬ8ЮЬШUJЪЬгЬгЬгЬ0ЩVKШKЪЮЬ	ЮЬЮЬШЪЮЬЮЬЮЬЮШKЪЬЮЬШKЪЬЮЬШKЪЬЮЬШKЪЬЮЬШKЪЬШKЪЬШKЪЮЬШЧKШЧЪбЬЮЬЮЬШJЩNKШJЩKKЪЬЮЬШKЪ
ЮЬЮЬЮЬЮЬЮЬШ8KШKШKШ
JЪЬDдЬЩPЪЮЬЮЬЮЬ
ЮШKЪЬЮЬШKЪЬЮЬШKЪЬЮЬШKШKШ,ЪЮЬЮШJЪЬз=ЬЩJЪЬ	Ш	KЪ
ЮЬЮЬЮЬ	ЮЬЮЬШ1KЪЬ,Ш,KЪЬШЧKЪЬ7Ш7KЪЮЬШЧKЪЮЬШЧJЪЬHЩHKШЧЪбЬЮЬЮЬШJЩMKШJЩLKЪЬЮЬШKЪЮЬЮЬЮ
ЬЮЬ
ЮЬЮЬШAKШKШKШ
JЪЬ*Щ*KЪ
ЮЬЮЬЮЬЮЬ*ЮЬШMKЪЬШJЪЬдЬ дЬ&ЩOKЪ
ЮЬЮЬЮЬ	ЮЬ ЮЬШ@Ъ	ЮЬЮЬЮ
ЬЮЬЮШ%KЪЬЮЬШKЪЬЮЬШKЪЬЮЬШKШKШ*KЪЬ/Ш/KЪЮЬШЧJЪЬUЩUKШЧЪбЬЮШJЩAKШJЩSЪЮЬЮЬЮЬ
ЮШKЪ
ЮЬЮЬЮЬ	ЮЬЮЬШ8KЪЮЬЮЬЮЬ
ЮЬЮЬЮЬЮЬЮЬШGKЪЮЬШЧЪЮЬЮЬЮЬ	ЮШЪЮЬЮЬЮЬЮШKЪЬЮЬШKШ+KШ)KШ*KЪЮЬШЧKЪЮЬШЧKШЧKЪЬЮЬШKШKШ	ЪЮЬЮЬЮШKЪЬЮЬШKШЪЮЬШЪЮЬШKЪЬШKЪЮЬЮЬЮЬЮЬШ:KЪЬШЧЪЮЬШKЪЬШKШKЪЬШKЪЮЬ	ЮЬШKЪЬШЧЧKЪЮЬШЧKЪЮЬШKШKШ ЧЧЪа
Щ
KШ ЧKЪЮЬШЧ ЕЧ    р  0М  