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