MODULE; IMPORT RedundantLSolve, JunoValue, NonLinearSolveRep; FROM RedundantLSolve IMPORT logWr; FROM JunoValue IMPORT Real; FROM NonLinearSolveRep IMPORT conAvail, conInUse, ConType; IMPORT IO, Wr, Fmt, Text, Stdio; FROM Thread IMPORT Alerted; <* FATAL Wr.Failure, Alerted *> VAR debug := 0; oneStep := FALSE; logFileName := "solver.log"; RedundantSolve
debug >= 1 => show input & solution debug >= 2 => show values & deltas at each iteration debug >= 3 => show values and roundoff errors at each iteration oneStep => take only one Newton step each solve; always report success logFile => name of file logging output is sent to; if NIL, output is sent to stdout
TYPE T = Real; PROCEDUREA constraint is considered satisfied if its error is at mostNewCon (type: ConType): Constraint = VAR res: Constraint; BEGIN IF conAvail # NIL THEN res := conAvail; conAvail := conAvail.availLink ELSE res := NEW(Constraint) END; res.availLink := conInUse; conInUse := res; res.type := type; RETURN res END NewCon; PROCEDURENewPlus (): Constraint = BEGIN RETURN NewCon(ConType.Plus) END NewPlus; PROCEDURENewMinus (): Constraint = BEGIN RETURN NewCon(ConType.Minus) END NewMinus; PROCEDURENewHalve (): Constraint = BEGIN RETURN NewCon(ConType.Halve) END NewHalve; PROCEDURENewTimes (): Constraint = BEGIN RETURN NewCon(ConType.Times) END NewTimes; PROCEDURENewSin (): Constraint = BEGIN RETURN NewCon(ConType.Sin) END NewSin; PROCEDURENewCos (): Constraint = BEGIN RETURN NewCon(ConType.Cos) END NewCos; PROCEDURENewAtan (): Constraint = BEGIN RETURN NewCon(ConType.Atan) END NewAtan; PROCEDURENewMultTan (): Constraint = BEGIN RETURN NewCon(ConType.MultTan) END NewMultTan; PROCEDURENewExp (): Constraint = BEGIN RETURN NewCon(ConType.Exp) END NewExp; PROCEDUREDispose () = VAR l := conInUse; BEGIN IF l # NIL THEN WHILE l.availLink # NIL DO l := l.availLink END; l.availLink := conAvail; conAvail := conInUse; conInUse := NIL END END Dispose; VAR MaxIterations := 20; MinDelta := 1.0e-7; RoundFudge := FLOAT(16.0, JunoValue.Real);
RoundFudge
times the estimated round-off error involved in computing it.
A change to a coordinate of the solution that has relative value less than
MinDelta
is considered tiny. When all changes are tiny, the iteration
stops.
CONST InitRows = 20; InitCols = 40; InitErrB = 100; VAR a := NEW(REF RedundantLSolve.Matrix, InitRows, InitCols + 1); (* matrix *) x := NEW(REF RedundantLSolve.Vector, InitCols); (* solution vector *) errorVec := NEW(REF RedundantLSolve.Vector, InitRows); (* one/constraint *) errb := NEW(REF RedundantLSolve.Vector, InitErrB); (* error bound *) CONST NameWidth = 9; Prec = 3; FieldWidth = Prec + 8; PROCEDUREShowVector ( name: TEXT; READONLY v: RedundantLSolve.Vector; size: CARDINAL) = BEGIN Wr.PutText(logWr, " "); IF size > 0 THEN Wr.PutText(logWr, Fmt.Pad(name, NameWidth - 2, align := Fmt.Align.Left)); FOR i := 0 TO size - 1 DO Wr.PutText(logWr, Fmt.Pad(Fmt.Real(v[i], Fmt.Style.Sci, prec := 3), FieldWidth)) END ELSE Wr.PutText(logWr, "<none>") END; Wr.PutChar(logWr, '\n'); END ShowVector; PROCEDUREEtpLogP0 (<*UNUSED*> true_cnt, nn, ghost_cnt, iterations: CARDINAL) =
true_cnt = # of true constraints; nn = # of true vars; ghost_cnt = # of
ghost vars and constraints, iterations
is the number of iterations.
BEGIN END EtpLogP0; PROCEDUREEvalRHS ( c: Constraint; READONLY v: ARRAY OF T; READONLY errb: ARRAY OF T; n: CARDINAL; VAR (*OUT*) res: T; VAR (*OUT*) err: T; supressNewline := FALSE) = (* Set "res" and "err" to the value and estimated round-off error of the right side of "c", assuming that "v" and "errb" give the value and round-off errors for the variables appearing in the right side of "c". The argument "res" may be aliased with an element of "v" that doesn't occur in the right side of "c", and "err" may be aliased to an element of "errb" that doesn't occur in the right side of "c". *) BEGIN WITH arg = c.arg, y = v[arg[1]], x = v[arg[2]], dely = errb[arg[1]], delx = errb[arg[2]] DO (* res := c.type(y,x) *) CASE c.type OF <*NOWARN*> | ConType.Plus => res := y + x; err := dely + delx | ConType.Minus => res := y - x; err := dely + delx | ConType.Halve => res := y * 0.5; err := dely * 0.5 | ConType.Times => res := y * x; err := dely * ABS(x) + delx * ABS(y) | ConType.Atan => res := JunoValue.Atan(y, x); err := (delx * ABS(y) + dely * ABS(x)) / ABS(x*x+y*y) | ConType.Sin => res := JunoValue.Sin(y); err := ABS(JunoValue.Cos(y)) * dely | ConType.Cos => res := JunoValue.Cos(y); err := ABS(JunoValue.Sin(y)) * dely | ConType.MultTan => WITH tanx = JunoValue.Tan(x), cosx = JunoValue.Cos(x) DO res := y * tanx; err := dely * ABS(tanx) + delx / (cosx * cosx) END | ConType.Exp => WITH expy = JunoValue.Exp(y) DO res := expy; err := expy * dely END END; IF c.type # ConType.Halve THEN err := err + JunoValue.HalfEps * ABS(res) END; IF debug >= 3 THEN ShowConstraint(c, v, n, supressNewline) END END END EvalRHS; PROCEDUREIndexVal (i, n: CARDINAL; READONLY v: ARRAY OF T): TEXT = BEGIN IF i >= n THEN RETURN Fmt.Real(v[i]) END; VAR res := ""; BEGIN LOOP res := Text.FromChar(VAL((i MOD 26) + ORD('a'), CHAR)) & res; IF i < 26 THEN EXIT END; i := i DIV 26 END; RETURN res END END IndexVal; PROCEDUREShowConstraint ( con: Constraint; READONLY v: ARRAY OF T; n: CARDINAL; supressNewline := FALSE) = BEGIN Wr.PutText(logWr, " " & IndexVal(con.arg[0], n, v) & " = "); CASE con.type OF <* NOWARN *> ConType.Plus => Wr.PutText(logWr, IndexVal(con.arg[1],n,v) & " + " & IndexVal(con.arg[2],n,v)) | ConType.Minus => Wr.PutText(logWr, IndexVal(con.arg[1],n,v) & " - " & IndexVal(con.arg[2],n,v)) | ConType.Halve => Wr.PutText(logWr, IndexVal(con.arg[1],n,v) & " / 2") | ConType.Times => Wr.PutText(logWr, IndexVal(con.arg[1],n,v) & " * " & IndexVal(con.arg[2],n,v)) | ConType.Atan => Wr.PutText(logWr, "ATAN(" & IndexVal(con.arg[1],n,v) & ", " & IndexVal(con.arg[2],n,v) & ")") | ConType.Sin => Wr.PutText(logWr, "SIN(" & IndexVal(con.arg[1],n,v) & ")") | ConType.Cos => Wr.PutText(logWr, "COS(" & IndexVal(con.arg[1],n,v) & ")") | ConType.MultTan => Wr.PutText(logWr, IndexVal(con.arg[1],n,v) & "* TAN("& IndexVal(con.arg[2],n,v) & ")") | ConType.Exp => Wr.PutText(logWr, "EXP(" & IndexVal(con.arg[1],n,v) & ")") END; IF NOT supressNewline THEN Wr.PutChar(logWr, '\n') END END ShowConstraint; PROCEDUREP ( nn, n: CARDINAL; VAR v: ARRAY OF T; READONLY c: ARRAY OF Constraint): BOOLEAN =
ON ENTRY: v[] ________ | | | True | | Vars | | | c[] |________| _____________ nn -> | | | | | | | | | Ghost | | Ghost | | Vars | | Constraints | | | | | |________| |_____________| n -> |........| | | <- ghost_cnt |........| | True | |________| | Constraints | | | | | | Consts | |_____________| |________| <- m (total # of constraints) MATRIX ORGANIZATION: x[] ____________________ __ | | |..| |____________________| |..| = unused |__| | <------ nn ------> | a[][] errorVec[] _______________________ __ ^ | | | |..| | | Ghost | | |..| | | Equations | | |..| | |____________________|__| |__| m | | | | | <- ghost_cnt | | True | | | | | | Equations | | | | V |____________________|__| |__| <- m | <------ nn + 1 -----> |
VAR m := NUMBER(c); (* number of constraints (total) *) ghost_cnt := n - nn; (* number of ghost constraints *) true_cnt := m - ghost_cnt; (* number of true constraints *) allUpdatesTiny: BOOLEAN; (* set by "UpdateVars" *) goodEnough: BOOLEAN; (* set by "EvalConstraints" *) PROCEDURE EvalConstraints(): T = (* Evaluate the ghost variables, and set "errorVec[i]" to the error in true constraint "i", for "i" in "[ghost_cnt..m-1]". Return the maximum absolute error of any true constraint. Also set "errb[i]" to the estimated round-off error in the value of "v[i]", for each "i" in "[0..n-1]", and set "goodEnough" to record whether all constraints were met as closely as can be expected given the rounding error involved in computing them. *) VAR result: T := 0.0; rhsval, rhserr, diff: T; BEGIN goodEnough := TRUE; IF debug >= 3 THEN IO.Put("Evaluating constraints:\n", logWr) END; FOR i := FIRST(v) TO LAST(v) DO errb[i] := JunoValue.HalfEps * ABS(v[i]) END; FOR i := 0 TO ghost_cnt - 1 DO WITH ci = c[i], arg0 = ci.arg[0] DO EvalRHS(ci, v, errb^, n, (*OUT*) v[arg0], (*OUT*) errb[arg0]) END END; FOR i := ghost_cnt TO m - 1 DO WITH ci = c[i], arg0 = ci.arg[0] DO EvalRHS(ci, v, errb^, n, (*OUT*) rhsval, (*OUT*) rhserr, supressNewline := (debug >= 3)); IF debug >= 3 THEN IO.Put(" = ", logWr); IO.PutReal(rhsval, logWr); IO.Put(" +- ", logWr); IO.PutReal(rhserr, logWr); IO.Put(" vs. ", logWr); IO.PutReal(v[arg0], logWr); IO.Put(" +- ", logWr); IO.PutReal(errb[arg0], logWr) END; diff := rhsval - v[arg0]; errorVec[i] := diff; diff := ABS(diff); IF diff > RoundFudge * (rhserr + errb[arg0]) THEN goodEnough := FALSE; IF debug >= 3 THEN IO.Put(" -> BAD", logWr) END END END; IF debug >= 3 THEN IO.Put("\n", logWr) END; IF diff > result THEN result := diff END END; RETURN result END EvalConstraints; PROCEDURE ZeroMatrix() = BEGIN FOR i := 0 TO m - 1 DO WITH row = a[i] DO FOR j := 0 TO nn - 1 DO row[j] := 0.0 END END END END ZeroMatrix; PROCEDURE BuildMatrix() = (* Fill in the matrix "a" to solve one step of the iteration for the constraints "c" using the current values of the variables "v". *) BEGIN ZeroMatrix(); (* compute gradients of ghost constraints *) FOR i := 0 TO ghost_cnt - 1 DO WITH con = c[i] DO GradCon(con, con.arg[0] - nn) END END; (* compute gradients of true constraints *) FOR i := ghost_cnt TO m - 1 DO WITH con = c[i] DO GradCon(con, i); GradVar(con.arg[0], i, -1.0); a[i, nn] := -(errorVec[i]) END END; END BuildMatrix; PROCEDURE GradCon(con: Constraint; i: CARDINAL) = (* Set "row" to the gradient of the right hand side of "con". *) BEGIN CASE con.type OF <* NOWARN *> | ConType.Plus => GradVar(con.arg[1], i, 1.0); GradVar(con.arg[2], i, 1.0) | ConType.Minus => GradVar(con.arg[1], i, 1.0); GradVar(con.arg[2], i, -1.0) | ConType.Halve => GradVar(con.arg[1], i, 0.5) | ConType.Times => WITH arg1 = con.arg[1], arg2 = con.arg[2] DO GradVar(arg1, i, v[arg2]); GradVar(arg2, i, v[arg1]) END | ConType.Sin => WITH arg1 = con.arg[1] DO GradVar(arg1, i, JunoValue.Cos(v[arg1])) END | ConType.Cos => WITH arg1 = con.arg[1] DO GradVar(arg1, i, -(JunoValue.Sin(v[arg1]))) END | ConType.Atan => (* d(atan2(y, x)) = d(atan(y/x)) = (1 / (1 + ((y/x)^2))) * ((x dy - y dx) / (x^2)) = (1 / (x^2 + y^2)) * (x dy - y dx) *) WITH arg1 = con.arg[1], arg2 = con.arg[2] DO VAR y := v[arg1]; x := v[arg2]; denom := x*x + y*y; BEGIN GradVar(arg1, i, x / denom); GradVar(arg2, i, -y / denom) END END | ConType.MultTan => (* d(x * tan(y)) = tan(y) dx + x * sec^2(y) dy = tan(y) dx + x * (1 + tan^2(y)) *) WITH arg1 = con.arg[1], arg2 = con.arg[2] DO VAR k := JunoValue.Tan(v[arg2]); BEGIN GradVar(arg1, i, k); GradVar(arg2, i, v[arg1] * (1.0 + k*k)) END END | ConType.Exp => WITH arg1 = con.arg[1] DO GradVar(arg1, i, JunoValue.Exp(v[arg1])) END END END GradCon; PROCEDURE GradVar(var, i: CARDINAL; k: T) = (* Add "k" to the coefficient of variable "var" in "row". However, if "var" is a ghost variable, we add "k" times the row in "a" for "var" to "row". *) BEGIN IF var < nn THEN (* true variable *) WITH row = a[i] DO row[var] := row[var] + k END ELSIF var < n THEN (* ghost variable *) WITH row = a[i], varRow = a[var - nn] DO FOR j := 0 TO nn DO row[j] := row[j] + (k * varRow[j]) END END END END GradVar; PROCEDURE UpdateVars() = BEGIN allUpdatesTiny := TRUE; FOR i := 0 TO nn - 1 DO WITH val = v[i] DO allUpdatesTiny := allUpdatesTiny AND (ABS(x[i]) < MinDelta*ABS(val)); val := val + x[i] END END END UpdateVars; PROCEDURE ShowInput() = PROCEDURE P(lo, hi: INTEGER; kind: TEXT) = BEGIN Wr.PutText(logWr, "RedundantSolve.P " & kind & " constraints:\n"); IF lo > hi THEN Wr.PutText(logWr, " <none>\n") ELSE FOR i := lo TO hi DO ShowConstraint(c[i], v, n) END END END P; BEGIN IF debug >= 1 THEN IO.Put("\n" & Fmt.Pad("", length := 45, padChar := '*') & "\n", logWr) END; P(0, ghost_cnt - 1, "ghost"); P(ghost_cnt, LAST(c), "true"); IO.Put("RedundantSolve.P hints:\n", logWr); IF nn > 0 THEN Wr.PutText(logWr, Fmt.Pad("", NameWidth)); FOR i := 0 TO nn - 1 DO Wr.PutText(logWr, Fmt.Pad(IndexVal(i, n, v), FieldWidth)) END; Wr.PutChar(logWr, '\n') END; ShowVector("Values", v, nn); Wr.Flush(logWr) END ShowInput; PROCEDUREShowValuesAndErrors (itNum: INTEGER) = BEGIN Wr.PutText(logWr, "Iteration " & Fmt.Int(itNum) & ":\n"); ShowVector("Values:", v, nn); WITH used_err_vec = SUBARRAY(errorVec^, ghost_cnt, true_cnt) DO ShowVector("Errors:", used_err_vec, true_cnt) END END ShowValuesAndErrors; PROCEDUREShowSolution () = BEGIN Wr.PutText(logWr, "Solution Found:\n"); ShowVector("Values", v, nn) END ShowSolution; PROCEDUREGrowArrays () = (* If necessary, make the arrays "x", "a", and "errorVec" larger. *) BEGIN IF nn > NUMBER(x^) OR m > NUMBER(errorVec^) THEN VAR n2 := MAX(nn, 2 * NUMBER(x^)); m2 := MAX(m, 2 * NUMBER(errorVec^)); BEGIN x := NEW(REF RedundantLSolve.Vector, n2); a := NEW(REF RedundantLSolve.Matrix, m2, n2 + 1); errorVec := NEW(REF RedundantLSolve.Vector, m2) END END; IF NUMBER(v) > NUMBER(errb^) THEN VAR n2 := MAX(NUMBER(v), 2 * NUMBER(errb^)); BEGIN errb := NEW(REF RedundantLSolve.Vector, n2) END END END GrowArrays; (* PROCEDURE P *) BEGIN IF logWr = NIL AND debug > 0 THEN IF logFileName # NIL THEN logWr := IO.OpenWrite(logFileName) ELSE logWr := Stdio.stdout END END; <* ASSERT NUMBER(x^) + 1 = NUMBER(a[0]) *> <* ASSERT NUMBER(errorVec^) = NUMBER(a^) *> GrowArrays(); IF debug >= 1 THEN ShowInput() END; VAR cnt := MaxIterations; error := EvalConstraints(); BEGIN IF true_cnt > 0 THEN TRY WHILE (NOT goodEnough) AND (cnt > 0) DO IF debug >= 2 THEN ShowValuesAndErrors(MaxIterations - cnt) END; BuildMatrix(); WITH matrix = SUBARRAY(a^, ghost_cnt, true_cnt) DO RedundantLSolve.P(true_cnt, nn, matrix, x^) END; IF debug >= 2 THEN ShowVector("Deltas", x^, nn) END; UpdateVars(); IF allUpdatesTiny THEN EXIT END; IF oneStep THEN RETURN TRUE END; error := EvalConstraints(); DEC(cnt) END; IF debug >= 1 THEN IF goodEnough THEN ShowSolution() ELSE IO.Put("No Solution Found.\n\n", logWr) END END; RETURN goodEnough FINALLY EtpLogP0(true_cnt, nn, ghost_cnt, MaxIterations-cnt); IF debug >= 2 THEN Wr.Flush(logWr) END END (* TRY *) END; (* IF *) RETURN TRUE END END P;
PROCEDURE SolveLoop(): BOOLEAN = CONST MaxCount = 20; VAR count := 0; goodEnough, allUpdatesTiny: BOOLEAN; BEGIN LOOP EVAL EvalConstraints(); IF goodEnough OR (count = MaxCount) THEN EXIT END; BuildMatrix(); EVAL RedundantLSolve(); UpdateVars(); IF allUpdatesTiny THEN EXIT END; INC(count) END; RETURN goodEnough END END;
BEGIN END RedundantSolve.