<<>> <> <> <> <<>> 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 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 sum: REAL ¬ b[i]; -- Dot row of TT with FOR j: NAT IN [mL-i..mL) DO sum ¬ sum - getT[j, nZ+i]* ENDLOOP; <<<> FOR i: NAT IN [nFR..n) DO sum: REAL ¬ g[i]; -- Dot row of AFXT with FOR j: NAT IN [0..mL) DO sum ¬ sum - getA[j, i]* ENDLOOP; }; NegativeLagrange: PROC RETURNS [k: NAT] ~ { mostNeg: REAL ¬ -eps; k ¬ n; FOR i: NAT IN [nFR..n) DO IF 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 => {}; -- In general algorithm, would update Cholesky R when i <= nZ.>> = 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..