-- Does one Newton-Raphson step.
-- constraints given the current positions of the fixed and tempfixed points.
=TRUE if all constraints are satisfied.
-- rounded to the nearest integer, have changed from their previous rounded values.
BEGIN
n, m: INT ← 0; -- the tableau a has n relevant rows and m relevant columns.
EvalWithGrad: PROC[ex: Se] RETURNS [results: Results, k: INTEGER] =
-- Evaluates a symbolic expression ex that returns a structure with
-- k (zero or more) real-valued simple components (no ropes, funvals, atoms).
-- Then adds k new rows to a[][] and b[]. Stores in b[] the
-- components of the result, and in a[][] the partial derivatives of each component
-- with respect ot all mobile variables entering into it.
BEGIN
WITH ex SELECT FROM
NIL => RETURN [0];
ATOM => {evaluate the atom, enumerate NumCells n result, push each};
NumCell => {must be constant; anyway, check it}
RopeCell=> error
FunPtr => error
LIST OF Se =>
evaluate op = first component
with op select from
+, - => evaluate each operand recursively, check if
same num. of components, add/subtract rows of a[][] and b[]
*, / => evaluate each operand; one must be single-valued
proceed as before
xcoord, ycoord => eval operand, select specified row
leftparen => eval operand
lefbrack => eval operand, make into list
comma => eval both sides, concat
etc
END;
EvalWithGrad:
PROC[
vex: Se] =
-- vex is an expression that returns two real values (coordinates of some 2-vector)
-- This procedure adds two new rows to the matrix a. It evaluates vex, and
-- saves the coordinates of the result in the two elements of b[].
-- The partial derivatives of each coordinate of vex
-- with respect to all mobile NumCells referenced by vex
-- are stored in the corresponding rows of a[][].
BEGIN
???
END;
EvNum:
PROC[
nex: Se] =
-- nex is an expression that returns one real value
-- This procedure evaluates nex, and adds one new row to the matrix a.
-- The value of nex is stored in
-- the b[] vector. The partial derivatives of nex
-- with respect to all mobile NumCells referenced by nex
-- are stored in the corresponding rows of a[][].
BEGIN
???
END;
EvRelVector:
PROC[
vex: Se, fex: Se] =
-- vex is an expression that returns one two real values (coordinates of some point)
-- fex is an expression that returns a frame (one to three vectors)
-- This procedure adds two new rows to the matrix a.
-- It computes u = (vex times the pseudo-inverse of the matrix of fex)
-- and stores the coordinates of u in the two elements of b[].
-- The partial derivatives of these coordinates
-- with respect to all mobile NumCells referenced by vex or fex
-- are stored in the corresponding rows of a[][].
BEGIN
EvVector[vex];
??? <evalute the frame,
END;
EvConstraint: PROC[cex: Se, fex: Se] =
-- cex is a constraint expression that returns one or two real values
-- "error functions" for the constraint)
-- fex is an expression that returns a frame (one to three vectors), or NIL
-- This procedure adds one or two new rows to the matrix a.
-- The error functions are stored in one or two elements of b[].
-- The partial derivatives of these error functions
-- with respect to all mobile NumCells referenced by cex or fex
-- are stored in the corresponding rows of a[][].
-- The tag of each row is set ot eh appropriate constraint type (eq, geq, leq).
BEGIN
???
END;
AddRow: PROC[i, j: [1..rowmax], coef: REAL] =
-- adds to row i of the matrix the contents of row j multiplied by coef.
-- The tag of each row is set ot eh appropriate constraint type (eq, geq, leq).
BEGIN
b[i] ← b[i] + coef * b[j];
FOR c: INTEGER [1..colmax] DO
a[i][c] ← a[i][c] + coef * a[j][c]
ENDLOOP
END;
PopRows: PROC [k: INTEGER] =
-- Deletes the last k rows of the matrix.
BEGIN
n ← n-k
END;
Constraint:
PROC[value:
REAL, tag: Tag] =
-- adds row to matrix for a new constraint whose current value is value
.
BEGIN
n ← n + 1; -- add new row
IF n > rowmax THEN SIGNAL TooManyConstraints[];
rowhead[n] ← [point: NIL, tag: tag];
IF tag=eq AND ABS[value] > 0.01 OR tag=geq AND value <-0.01 THEN
{success ← FALSE};
FOR j: INT IN [1..m] DO a[n][j] ← 0.0 ENDLOOP;
b[n] ← - value;
END;
Partial:
PROC[point: PointPtr, xCoef, yCoef:
REAL] =
-- Adds to the matrix one or two new entries, corresponding to the partial derivatives
-- of the most recently entered error function
-- with respect to the x and y coordinates of the given point.
BEGIN
IF
NOT point.mobile
THEN
RETURN;
IF
ABS[xCoef] > .000001
THEN
BEGIN
IF point.xCol = 0 THEN -- add new column:
{m ← m + 1;
IF m > colmax THEN SIGNAL TooManyConstrainedPoints[];
FOR i: INT IN [1..n] DO a[i][m] ← 0.0 ENDLOOP;
point.xCol ← m;
colhead[m] ← [point:point, tag: x]};
a[n][point.xCol] ← a[n][point.xCol] + xCoef
END;
IF
ABS[yCoef] > .000001
THEN
BEGIN
IF point.yCol = 0 THEN -- add new column:
{m ← m + 1;
IF m > colmax THEN SIGNAL TooManyConstrainedPoints[];
FOR i: INT IN [1..n] DO a[i][m] ← 0.0 ENDLOOP;
point.yCol ← m;
colhead[m] ← [point:point, tag: y]};
a[n][point.yCol] ← a[n][point.yCol] + yCoef
END;
END;
EnterVer:
PROC[i, j: PointPtr] =
BEGIN
Constraint[i.x - j.x, eq];
Partial[i, 1, 0]; Partial[j, -1, 0]
END;
EnterHor:
PROC[i, j: PointPtr] =
BEGIN
Constraint[i.y - j.y, eq];
Partial[i, 0, 1]; Partial[j, 0, -1]
END;
EnterPara:
PROC[i, j, k, l: PointPtr] =
BEGIN
Constraint[(j.x - i.x)*(k.y - l.y) + (l.x - k.x)*(j.y - i.y), eq];
Partial[i, l.y - k.y, k.x - l.x];
Partial[j, k.y - l.y, l.x - k.x];
Partial[k, i.y - j.y, j.x - i.x];
Partial[l, j.y - i.y, i.x - j.x]
END;
EnterCong:
PROC[i, j, k, l: PointPtr, frame: Frame] =
BEGIN
IF frame # [
NIL,
NIL,
NIL]
AND frame.xP#
NIL
AND frame.yP#
NIL
THEN
-- relativized congruence
{ux: REAL = frame.xP.x - frame.org.x;
uy: REAL = frame.xP.y - frame.org.y;
vx: REAL = frame.yP.x - frame.org.x;
vy: REAL = frame.yP.y - frame.org.y;
-- The following computations essentially multiply the coordinates of
-- the constrained points by the inverse of the frame matrix,
-- except that the division by the frame's determinant is left out.
-- This still preserves the zeros of the constraint, and is MUCH easier to
-- compute and differentiate. It also avoids division by zero if the
-- frame becomes singular. However, in that case
-- it will consider the constraint as being satisfied.
iju: REAL = vy*(j.x-i.x) - vx*(j.y - i.y); -- coords of (j-i) rel frame.
ijv: REAL = -uy*(j.x-i.x) + ux*(j.y - i.y);
klu: REAL = vy*(l.x-k.x) - vx*(l.y - k.y); -- coords of (l-k) rel frame
klv: REAL = -uy*(l.x-k.x) + ux*(l.y - k.y);
error: REAL = 0.5 * (iju*iju + ijv*ijv- klu*klu - klv*klv); -- error function
dijx: REAL = vy*iju - uy*ijv; -- der. of error w.r.t. (j.x-i.x)
dijy: REAL = -vx*iju + ux*ijv;
dklx: REAL = -vy*klu + uy*klv;
dkly: REAL = vx*klu - ux*klv;
dvx: REAL = -iju*(j.y-i.y) + klu*(l.y - k.y); -- der. w.r.t. vx
dvy: REAL = iju*(j.x-i.x) - klu*(l.x-k.x);
dux: REAL = ijv*(j.y - i.y) - klv*(l.y - k.y); -- der. w.r.t. ux
duy: REAL = -ijv*(j.x-i.x) + klv*(l.x-k.x);
Constraint[error, eq];
Partial[i, -dijx, -dijy];
Partial[j, dijx, dijy];
Partial[k, -dklx, -dkly];
Partial[l, dklx, dkly];
Partial[frame.xP, dux, duy];
Partial[frame.yP, dvx, dvy];
Partial[frame.org, -dux-dvx, -duy-dvy]} -- Phew!
ELSE
-- frame is orthogonal; absolute congruence
{ijx: REAL = j.x - i.x;
ijy: REAL = j.y - i.y;
klx: REAL = l.x - k.x;
kly: REAL = l.y - k.y;
Constraint[0.5 * (ijx*ijx + ijy*ijy - klx*klx - kly*kly), eq];
Partial[i, -ijx, -ijy];
Partial[j, ijx, ijy];
Partial[k, klx, kly];
Partial[l, -klx, -kly]}
END;
EnterPerp:
PROC[i, j, k, l: PointPtr, frame: Frame] =
BEGIN
IF frame # [
NIL,
NIL,
NIL]
AND frame.xP#
NIL
AND frame.yP#
NIL
THEN
-- relativized perpendicularity
{ux: REAL = frame.xP.x - frame.org.x; -- frame vectors.
uy: REAL = frame.xP.y - frame.org.y;
vx: REAL = frame.yP.x - frame.org.x;
vy: REAL = frame.yP.y - frame.org.y;
-- The following computations essentially multiply the coordinates of
-- the constrained points by the inverse of the frame matrix,
-- except that the division by the frame's determinant is left out.
-- This still preserves the zeros of the constraint, and is MUCH easier to
-- compute and differentiate. It also avoids division by zero if the
-- frame becomes singular. However, in that case
-- it will consider the constraint as being satisfied.
iju: REAL = vy*(j.x-i.x) - vx*(j.y - i.y); -- coords of (j-i) rel frame.
ijv: REAL = -uy*(j.x-i.x) + ux*(j.y - i.y);
klu: REAL = vy*(l.x-k.x) - vx*(l.y - k.y); -- coords of (l-k) rel frame
klv: REAL = -uy*(l.x-k.x) + ux*(l.y - k.y);
error: REAL = iju*klu+ijv*klv; -- error function for this constraint
dijx: REAL = vy*klu - uy*klv; -- der. of error w.r.t. (j.x-i.x)
dijy: REAL = -vx*klu + ux*klv;
dklx: REAL = iju*vy - ijv*uy;
dkly: REAL = -iju*vx + ijv*ux;
dvx: REAL = -(j.y-i.y)*klu - iju*(l.y-k.y); -- der. w.r.t. vx
dvy: REAL = (j.x-i.x)*klu + iju*(l.x-k.x);
dux: REAL = -(j.y-i.y)*klv - ijv*(l.y-k.y); -- der. w.r.t. ux
duy: REAL = (j.x-i.x)*klv + ijv*(l.x-k.x);
Constraint[error, eq];
Partial[i, -dijx, -dijy];
Partial[j, dijx, dijy];
Partial[k, -dklx, -dkly];
Partial[l, dklx, dkly];
Partial[frame.xP, dux, duy];
Partial[frame.yP, dvx, dvy];
Partial[frame.org, -dux-dvx, -duy-dvy]} -- Phew!
ELSE
-- frame is orthogonal; absolute perpendicularity
{Constraint[(j.x - i.x)*(l.x - k.x) + (j.y - i.y)*(l.y - k.y), eq];
Partial[i, k.x - l.x, k.y - l.y];
Partial[j, l.x - k.x, l.y - k.y];
Partial[k, i.x - j.x, i.y - j.y];
Partial[l, j.x - i.x, j.y - i.y]}
END;
EnterAt:
PROC[pp: PointPtr, x, y:
REAL, frame: Frame] =
BEGIN
xT, yT:
REAL;
IF frame # [
NIL,
NIL,
NIL]
THEN
{mFrame ← Alg.GetFrameMatrix [frame, mFrame];
[xT, yT] ← Alg.TransformPoint[x, y, mFrame]}
ELSE
{xT ← x; yT ← y};
Constraint[xT - pp.x, eq];
Partial[pp, -1, 0];
IF frame # [
NIL,
NIL,
NIL]
THEN
-- enter derivatives with respect to frame
{Partial[frame.org, 1, 0];
IF frame.xP # NIL THEN
{Partial[frame.xP, x, 0]; Partial[frame.org, -x, 0];
IF frame.yP # NIL THEN
{Partial[frame.yP, y, 0]; Partial[frame.org, -y, 0]}
ELSE
{Partial[frame.xP, 0, -y]; Partial[frame.org, 0, y]}}};
Constraint[yT - pp.y, eq];
Partial[pp, 0, -1];
IF frame # [
NIL,
NIL,
NIL]
THEN
-- enter derivatives with respect to frame
{Partial[frame.org, 0, 1];
IF frame.xP # NIL THEN
{Partial[frame.xP, 0, x]; Partial[frame.org, 0, -x];
IF frame.yP # NIL THEN
{Partial[frame.yP, 0, y]; Partial[frame.org, 0, -y]}
ELSE
{Partial[frame.xP, y, 0]; Partial[frame.org, -y, 0]}}}
END;
EnterCcw:
PROC[i, j, k: PointPtr, frame: Frame] =
BEGIN
fudge: REAL = 500.0; -- Greg's mysterious fudge factor
IF frame # [NIL, NIL, NIL] THEN ERROR;
Constraint[(j.y-i.y)*(k.x-i.x) - (j.x-i.x)*(k.y-i.y) + fudge, geq];
Partial[i, k.y-j.y, j.x-k.x];
Partial[j, i.y-k.y, k.x-i.x];
Partial[k, j.y-i.y, i.x-j.x]
END;
-- Prepare points for iteration: reset xCol and yCol and save old coordinates:
BEGIN
p: PointPtr ← points;
UNTIL p = NIL DO
p.oldx ← Real.RoundLI[p.x];
p.oldy ← Real.RoundLI[p.y];
p.xCol ← 0; p.yCol ← 0;
p ← p.link
ENDLOOP;
END;
-- Fill in tableau a[][] and constant column b[] by enumerating current constraints:
success ← TRUE; impossible ← FALSE;
BEGIN
c: Stor.ConstrPtr ← constrs;
UNTIL c = NIL DO
WITH c SELECT FROM
cc: Stor.HorPtr => EnterHor[cc.i, cc.j];
cc: Stor.VerPtr => EnterVer[cc.i, cc.j];
cc: Stor.ParaPtr => EnterPara[cc.i, cc.j, cc.k, cc.l];
cc: Stor.CongPtr => EnterCong[cc.i, cc.j, cc.k, cc.l, cc.frame];
cc: Stor.PerpPtr => EnterPerp[cc.i, cc.j, cc.k, cc.l, cc.frame];
cc: Stor.AtPtr => EnterAt[cc.p, cc.x, cc.y, cc.frame];
cc: Stor.CcwPtr => EnterCcw[cc.i, cc.j, cc.k, cc.frame];
ENDCASE => ERROR;
c ← c.link
ENDLOOP
END;
IF success THEN RETURN;
-- Now invert matrix:
BEGIN
i, j, r, c:
INT;
pp:
REAL;
-- r is the row in which we are pivoting.
-- c 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 r
IN [1..n]
DO
IF rowhead[r].tag = eq
OR rowhead[r].tag = geq
AND b[r] < -.01
THEN
BEGIN
-- Set c so a[r,c] is largest of a[r,1], a[r, 2], a[r, 3], excluding
-- columns in which we have already pivoted.
pp ← 0;
c ← 0;
FOR j IN [1 .. m] DO
IF ABS[a[r][j]] >= pp AND colhead[j].tag IN [x .. y] THEN
{c ← j; pp ← ABS[a[r][c]]}
ENDLOOP;
-- We will pivot at a[r,c], if it exists and is not too small, and is legal.
-- First check that it is not too small:
IF c = 0 OR ABS[a[r][c]] < .001 THEN
IF ABS[b[r]] > .01
THEN {impossible ← TRUE; LOOP} -- this row is an unsatisfiable constraint
ELSE LOOP; -- row is identically zero; constraint redundant.
-- Next we must check that pivoting at a[r, c] will not ruin any
-- inequalities that are currently satisfied. If there are any such inequalities,
-- we change r to the index of one of them
-- (namely, the first that would become unsatisfied),
-- and pivot there instead.
BEGIN
-- adjust r to account for inequalities:
kk: INTEGER ← r; -- save r
FOR i: INT IN [1..n] DO
IF i # kk AND rowhead[i].tag = geq AND a[i][1] >= 0.0
AND a[i][c] < 0.0 AND b[i] - a[i][c] * b[r] / a[r][c] < 0.0 THEN r ← i
ENDLOOP
END;
-- Exchange row and column labels.
BEGIN
temp: TableauLabel = colhead[c];
colhead[c] ← rowhead[r];
rowhead[r] ← temp
END;
-- Do the pivoting.
-- >>> Possible bug here: what if a[r][c]<0? Dividing by it would reverse the
-- sign of the inequality. Is that OK?
pp ← 1.0 / a[r][c]; a[r][c] ← 1.0;
-- divide everything in pivot row by the pivot element:
b[r] ← b[r]*pp;
FOR j IN [1..m] DO
a[r][j] ← a[r][j] * pp
ENDLOOP;
-- subtract from other rows:
FOR i IN [1..n] DO
IF i # r THEN
{FOR j IN [1..m] DO
IF j # c THEN -- for each a[i,j] outside the pivot row and column
a[i][j] ← a[i][j] - a[i][c] * a[r][j]; -- note that a[r,j] was already * pp.
ENDLOOP;
b[i] ← b[i] - a[i][c] * b[r]}
ENDLOOP;
-- Finally process pivot column:
FOR i IN [1..n] DO
IF i # r THEN a[i][c] ← -a[i][c] * pp
ENDLOOP;
END
ENDLOOP;
END;
-- Now move the points, and check if any really moved:
moved ← FALSE;
BEGIN
p: Stor.PointPtr;
FOR i: INT IN [1..n] DO
IF rowhead[i].tag IN [x .. y] THEN-- row was neither redundant nor unsatisfiable
{p ← rowhead[i].point;
IF rowhead[i].tag = x THEN
{p.x ← p.x + b[i]; IF p.oldx # Real.RoundLI[p.x] THEN moved ← TRUE}
ELSE
{p.y ← p.y + b[i]; IF p.oldy # Real.RoundLI[p.y] THEN moved ← TRUE}};
ENDLOOP;
END;