-- December 10, 1982 2:25 pm
-- program junoSolverImplB.mesa, coded July 1981 by Greg Nelson
-- This is the solver for an interactive graphics program that allows a user to
-- specify the positions of points by declaring geometrical contraints that
-- the positions are to satisfy. The program solves constraints (which can be
-- viewed as simultaneous polynomial equations) by an n-dimensional version of
-- Newton's method for finding the root of a differentiable function.
-- Last Edited by: Gnelson, October 11, 1983 7:00 pm
DIRECTORY RealFns, JunoStorage, JunoSolver, JunoGraphics, Real, IO, JunoButtons;
JunoSolverImplB: PROGRAM
IMPORTS RealFns, JunoStorage, JunoGraphics, Real
EXPORTS JunoSolver =
BEGIN OPEN JunoStorage, JunoSolver, JG: JunoGraphics;
displayingMotions: PUBLIC BOOL ← FALSE;
loggingPivots: BOOL ← FALSE;
rowmax: INTEGER = 100;
colmax: INTEGER = 100;
Tableau: TYPE = ARRAY[1..rowmax] OF ARRAY[1..colmax] OF REAL;
n,m: PUBLIC INT; -- the tableau a has n relevant rows and m relevant columns.
a: REF Tableau ← NEW[Tableau];
Tag: TYPE = {x, y, eq, geq};
TableauLabel: TYPE = RECORD [point: PointPtr, tag: Tag];
-- each rows and column of the matrix represents a single scalar variable.
-- If the row or column is labelled with the TableauLabel (p, x),
-- then the scalar variable that the row or column represents is
-- the x coordinate of the point p; similarly (p, y) means the y coordinate.
-- The label (*, eq) means the scalar variable is a "slack variable" associated
-- with some constraint; the variable should have the value zero in any
-- solution. The label (*, geq) menas the scalar variable should have a
-- value greater than or equal to zero in any solution.
rowhead: ARRAY[1..rowmax] OF TableauLabel;
colhead: ARRAY[1..colmax] OF TableauLabel;
TooManyConstraints: SIGNAL = CODE;
TooManyConstrainedPoints: SIGNAL = CODE;
Square: PROCEDURE[x:REAL] RETURNS [REAL] = BEGIN RETURN [x*x] END;
Sqrt: PROCEDURE[x:REAL] RETURNS [REAL] = BEGIN RETURN [RealFns.SqRt[x]] END;
Solve: PUBLIC PROC[epsilon: REAL] RETURNS [success: BOOLEAN] =
{n: INT ← 0;
SavePositions[];
[] ← NewtonRaphson[];
WHILE n < 20 AND SomePointMoved[] DO
IF displayingMotions THEN DisplayMotions[];
n ← n + 1;
SavePositions[];
[] ← NewtonRaphson[]
ENDLOOP;
RETURN [n < 20]};
SavePositions: PROC =
{p: PointPtr ← pointLpad.link;
UNTIL p = pointRpad DO p.oldx ← Real.Fix[p.x]; p.oldy ← Real.Fix[p.y]; p ← p.link ENDLOOP};
SomePointMoved: PROC RETURNS [BOOL] =
{p: PointPtr ← pointLpad.link;
UNTIL p = pointRpad DO
IF p.oldx # Real.Fix[p.x] OR p.oldy # Real.Fix[p.y] THEN RETURN [TRUE];
p ← p.link
ENDLOOP;
RETURN [FALSE]};
DisplayMotions: PROC =
{p: PointPtr ← pointLpad.link;
UNTIL p = pointRpad DO
IF p.oldx # Real.Fix[p.x] OR p.oldy # Real.Fix[p.y]
THEN JG.DrawEdge[p.oldx, p.oldy, p.x, p.y];
p ← p.link
ENDLOOP};
NewtonRaphson: PROC RETURNS [success: BOOLEAN] =
BEGIN
NewtonStep: PROC[error: REAL, gradient: TangentVector, tag: Tag] =
-- processes one constraint whose current value is ERROR and whose current
-- gradient is GRADIENT. Processes means adds row to matrix.
BEGIN
IF gradient = NIL THEN RETURN;
n ← n + 1; -- add new row
IF n > rowmax THEN SIGNAL TooManyConstraints[];
rowhead[n] ← [point: NIL, tag: tag];
FOR j: INTEGER IN [2..m] DO a[n][j] ← 0.0 ENDLOOP;
a[n][1] ← - error;
UNTIL gradient = NIL DO
IF ABS[gradient.x] > .000001 AND (NOT gradient.head.fixed)
AND (NOT gradient.head.tempfixed) THEN
{IF gradient.head.xheader = 0
THEN -- add new column:
{m ← m + 1;
IF m > colmax THEN SIGNAL TooManyConstrainedPoints[];
FOR i: INTEGER IN [1..n] DO a[i][m] ← 0.0 ENDLOOP;
gradient.head.xheader ← m;
colhead[m] ← [point:gradient.head, tag: x]};
a[n][gradient.head.xheader] ← a[n][gradient.head.xheader] + gradient.x};
IF ABS[gradient.y] > .000001 AND (NOT gradient.head.fixed)
AND (NOT gradient.head.tempfixed) THEN
{IF gradient.head.yheader = 0
THEN -- add new column:
{m ← m + 1;
IF m > colmax THEN SIGNAL TooManyConstrainedPoints[];
FOR i: INTEGER IN [1..n] DO a[i][m] ← 0.0 ENDLOOP;
gradient.head.yheader ← m;
colhead[m] ← [point:gradient.head, tag: y]};
a[n][gradient.head.yheader] ← a[n][gradient.head.yheader] + gradient.y};
gradient ← gradient.tail;
ENDLOOP;
END;
ConsTv: PROC[head:PointPtr, x:REAL, y:REAL, tail:TangentVector] RETURNS [TangentVector] =
{tv:TangentVector ← NewTv[];
tv.head ← head;
tv.x ← x;
tv.y ← y;
tv.tail ← tail;
RETURN [tv]};
GradHor: PROC[p1, p2: PointPtr] RETURNS [tv:TangentVector] =
-- return gradient of function p1.y - p2.y:
{tv ← ConsTv[head:p1, x:0, y:1, tail:ConsTv[head:p2, x:0, y:-1, tail:NIL]]};
GradVer: PROC[p1, p2: PointPtr] RETURNS [tv:TangentVector] =
-- return gradient of function p1.x - p2.x:
{tv ← ConsTv[p1, 1, 0, ConsTv[p2, -1, 0, NIL]]};
GradCong: PROC[p1, p2, p3, p4: PointPtr] RETURNS [tv:TangentVector] =
{tv ← ConsTv[p1, p1.x - p2.x, p1.y - p2.y,
ConsTv[p2, p2.x - p1.x, p2.y - p1.y,
ConsTv[p3, p4.x - p3.x, p4.y - p3.y,
ConsTv[p4, p3.x - p4.x, p3.y - p4.y, NIL]]]]};
GradLin: PROC[pi, pj, pk, pl: PointPtr] RETURNS [tv: TangentVector] =
{tv ← ConsTv[pi, pl.y - pk.y, pk.x - pl.x,
ConsTv[pj, pk.y - pl.y, pl.x - pk.x,
ConsTv[pk, pi.y - pj.y, pj.x - pi.x,
ConsTv[pl, pj.y - pi.y, pi.x - pj.x, NIL]]]]};
GradCC: PROC[p1, p2, p3: PointPtr] RETURNS [tv: TangentVector] =
{tv ← ConsTv[p1, p3.y - p2.y, p2.x-p3.x,
ConsTv[p2, p1.y-p3.y, p3.x-p1.x,
ConsTv[p3, p2.y-p1.y,p1.x-p2.x, NIL]]]};
p: PointPtr ← pointLpad.link;
UNTIL p = pointRpad DO p.xheader ← 0; p.yheader ← 0; p ← p.link ENDLOOP;
success ← TRUE;
n ← 0;
m ← 1; -- matrix has n rows and m columns.
-- column 1 is the constant column.
BEGIN -- handle counter-clockwise constraints:
p: CCPtr ← ccLpad.link;
UNTIL p = ccRpad DO
NewtonStep[(p.j.y - p.i.y) * (p.k.x - p.i.x) - (p.j.x - p.i.x) * (p.k.y - p.i.y) + 500.0,
GradCC[p.i, p.j, p.k], geq];
p ← p.link;
ENDLOOP;
END;
BEGIN -- handle string constraints:
p: StringPtr ← stringLpad.link;
UNTIL p = stringRpad DO
NewtonStep [p.b4.x - p.b3.x - p.width, GradVer[p.b4, p.b3], eq];
NewtonStep [p.b4.y - p.b3.y, GradHor[p.b4, p.b3], eq];
p ← p.link;
ENDLOOP;
END;
BEGIN -- handle horizontal constraints:
p: HorPtr ← horLpad.link;
UNTIL p = horRpad DO
NewtonStep[p.i.y - p.j.y, GradHor[p.i, p.j], eq];
p ← p.link
ENDLOOP;
END;
BEGIN -- handle vertical constraints:
p: VerPtr ← verLpad.link;
UNTIL p = verRpad DO
NewtonStep[p.i.x - p.j.x, GradVer[p.i, p.j], eq];
p ← p.link;
ENDLOOP;
END;
BEGIN -- handle parallel constraints:
p: LinPtr ← lineLpad.link;
UNTIL p = lineRpad DO
NewtonStep[(p.j.x - p.i.x)*(p.k.y - p.l.y) + (p.l.x - p.k.x)*(p.j.y - p.i.y),
GradLin[p.i, p.j, p.k, p.l], eq];
p ← p.link;
ENDLOOP;
END;
BEGIN -- handle congruence constraints:
p: CongPtr ← congLpad.link;
UNTIL p = congRpad DO
NewtonStep[0.5 * (Square[p.j.x - p.i.x] + Square[p.j.y - p.i.y]
- Square[p.l.x - p.k.x] - Square[p.l.y - p.k.y]),
GradCong[p.i, p.j, p.k, p.l], eq];
p ← p.link;
ENDLOOP;
END;
-- Now invert matrix, and move points:
BEGIN
i, j, k, l: INTEGER;
pp: REAL;
-- k is the row in which we are pivoting.
-- l is the column in which we are pivoting.
-- i and j are miscellaneous row and column indices respectively
-- p is the reciprocal of the pivot element; also used as temp for swapping.
FOR k IN [1..n] DO
IF rowhead[k].tag = eq OR rowhead[k].tag = geq AND a[k][1] < -.01
THEN {-- set l so a[k,l] is largest of a[k,1], a[k, 2], a[k, 3], excluding
-- columns in which we have already pivoted.
pp ← 0;
l ← 0;
FOR j IN [2 .. m]
DO IF ABS[a[k][j]] >= pp AND colhead[j].tag IN [x .. y]
THEN {l ← j; pp ← ABS[a[k][l]]}
ENDLOOP;
-- We will pivot at a[k,l], if it exists and is not too small, and is legal.
-- First check that it is not too small:
IF l = 0 OR ABS[a[k][l]] < .001 THEN
IF ABS[a[k][1]] > .01
-- then this row is an unsatisfiable constraint so:
THEN {LOOP}
ELSE LOOP; -- row is identically zero; constraint redundant.
-- Next we must check that pivoting at a[k, l] will not ruin any
-- inequalities that are currently satisfied. If there are any such inequalities,
-- we change k to the index of one of them (namely, the first that would become
-- unsatisfied), and pivot there instead. That is the function of the following block:
{ -- adjust k to account for inequalities:
kk: INTEGER ← k; -- save k
FOR i: INT IN [1..n] DO
IF i # kk AND rowhead[i].tag = geq AND a[i][1] >= 0.0
AND a[i][l] < 0.0 AND a[i][1] - a[i][l] * a[k][1] / a[k][l] < 0.0
THEN k ← i
ENDLOOP};
{temp:TableauLabel ← colhead[l]; colhead[l] ← rowhead[k]; rowhead[k] ← temp};
pp ← 1.0 / a[k][l]; a[k][l] ← 1.0;
-- divide everything in pivot row by the pivot element:
FOR j IN [1..m] DO a[k][j] ← a[k][j] * pp ENDLOOP;
FOR i IN [1..n] DO
IF i # k THEN
FOR j IN [1..m] DO
IF j # l THEN -- for each a[i,j] outside the pivot row and column
a[i][j] ← a[i][j] - a[i][l] * a[k][j]; -- note that a[k,j] was already * pp.
ENDLOOP ENDLOOP;
-- Finally process pivot column:
FOR i IN [1..n] DO IF i # k THEN a[i][l] ← -a[i][l] * pp ENDLOOP;
}
ENDLOOP;
-- now move the points:
FOR i IN [1..n] DO
IF rowhead[i].tag IN [x .. y]
-- as it will be except for redundant rows
-- (or unsatisfiable rows)
THEN
IF rowhead[i].tag = x
THEN rowhead[i].point.x ← rowhead[i].point.x + a[i][1]
ELSE rowhead[i].point.y ← rowhead[i].point.y + a[i][1];
ENDLOOP;
END;
END;
END.