QPSetupImpl.mesa
Copyright Ó 1990 by Xerox Corporation. All rights reserved.
Ken Shoemake, June 15, 1990 2:20 am PDT
DIRECTORY
PBasics, QPSetup, QPSolve, RealFns, Rope;
QPSetupImpl:
CEDAR
PROGRAM
IMPORTS PBasics, QPSolve, RealFns, Rope
EXPORTS QPSetup
= BEGIN
OPEN QPSolve, QPSetup;
SparseMatrix: TYPE ~ REF SparseMatrixRep;
SparseMatrixRep: TYPE ~ RECORD [m: NAT, s: SEQUENCE len: NAT OF TermList];
FindFreeVars:
PUBLIC
PROC [cycleEqns, extraEqns: EqnList, nEqns, nVars:
NAT]
RETURNS [rowOf: IVector]
Return rowOf filled with index of row for free vars, noRow for rest of vars.
Index means little now; just needs to be different from noRow.
~ {
rowOf ← NEW[IVectorRep[nVars]];
FOR col: NAT IN [0..nVars) DO rowOf[col] ← noRow ENDLOOP;
FOR eqns: EqnList ← cycleEqns, eqns.rest
UNTIL eqns =
NIL
DO
rowOf[eqns.first.first.eData.var] ← 0; -- first var of cycle eqn is back edge
ENDLOOP;
<< Need a QR algorithm here to accomodate extra eqns, ensure independence. >>
};
FindComponents:
PUBLIC
PROC [g: Graph]
RETURNS [compList: ComponentList ←
NIL]
Return list of biconnected components of graph, with necessary cycle equations.
~ {
edgeStack: EdgeList ← NIL;
eqnStack: EqnList ← NIL;
Stack:
PROC [edge: Edge]
RETURNS [EdgeList]
~ {RETURN[edgeStack ← CONS[edge, edgeStack]]};
UnStackComponent:
PROC [bottomEdge: EdgeList, bottomEqn: EqnList]
~ {
component: Component ← NEW[ComponentRep ←
[join: NIL, weight: 1, edges: edgeStack, cycleEqns: eqnStack, extraEqns: NIL]];
edgeStack ← bottomEdge.rest; bottomEdge.rest ← NIL;
IF eqnStack = bottomEqn
THEN component.cycleEqns ← NIL
ELSE {
eqnStack ← bottomEqn;
FOR eqns: EqnList ← component.cycleEqns, eqns.rest
UNTIL eqns =
NIL
DO
IF eqns.rest = bottomEqn THEN {eqns.rest ← NIL; EXIT};
ENDLOOP;
};
FOR edges: EdgeList ← component.edges, edges.rest
UNTIL edges =
NIL
DO
edge: Edge ← edges.first;
edge.eData.join ← component;
ENDLOOP;
compList ← CONS[component, compList];
};
FormEquation:
PROC [e: Edge]
RETURNS [eqn: TermList]
~ {
v: Vertex ← e.thisEnd;
w: Vertex ← e.thatEnd;
eqn ← NIL;
FOR t: Vertex ← v.tree.thisEnd, v.tree.thisEnd
WHILE v.visited > w.visited
DO
eqn ← CONS[Term[eData: v.tree.eData, coef: v.tree.dir], eqn];
v ← t;
ENDLOOP;
FOR t: Vertex ← w.tree.thisEnd, w.tree.thisEnd
WHILE w.visited > v.visited
DO
eqn ← CONS[Term[eData: w.tree.eData, coef: -w.tree.dir], eqn];
w ← t;
ENDLOOP;
eqn ← CONS[Term[eData: e.eData, coef: e.dir], eqn]; -- back edge is first, for FindFreeVars
};
BiConn:
PROC [g: Graph, treeEdge: Edge]
RETURNS [vMin:
INT]
~ {
from: Vertex ← treeEdge.thisEnd;
v: Vertex ← treeEdge.thatEnd;
v.tree ← treeEdge;
v.visited ← vMin ← g.order ← g.order+1;
FOR edges: EdgeList ← v.adj, edges.rest
UNTIL edges =
NIL
DO
edge: Edge ← edges.first;
w: Vertex ← edge.thatEnd;
IF w.visited = 0
THEN {
bottomEdge: EdgeList ← Stack[edge];
bottomEqn: EqnList ← eqnStack;
wMin: INT ← BiConn[g, edge];
vMin ← MIN[vMin, wMin];
IF wMin >= v.visited THEN UnStackComponent[bottomEdge, bottomEqn];
}
ELSE {
IF w = from THEN LOOP;
vMin ← MIN[vMin, w.visited];
IF v.visited > w.visited
THEN {
[] ← Stack[edge];
eqnStack ← CONS[FormEquation[edge], eqnStack];
};
};
ENDLOOP;
};
FOR vL: VertexList ← g.vList.rest, vL.rest UNTIL vL.first = NIL DO vL.first.visited ← 0 ENDLOOP;
g.order ← 0;
FOR vL: VertexList ← g.vList.rest, vL.rest
UNTIL vL.first =
NIL
DO
IF vL.first.visited = 0 THEN [] ← BiConn[g, NEW[EdgeRep ← [NIL, vL.first, NIL, 1.0]]];
ENDLOOP;
};
TangleOf:
PROC [x: Component]
RETURNS [z: Component]
~ {
Find in union forest with path compression.
t: Component;
FOR z ← x, z.join DO IF z.join = NIL THEN EXIT ENDLOOP;
UNTIL x.join = NIL DO t ← x; x ← x.join; t.join ← z ENDLOOP;
};
Entangle:
PROC [x, y: Component]
~ {
Make union in union forest with weight balancing and path compression.
i, j, z, t: Component;
FOR i ← x, i.join DO IF i.join = NIL THEN EXIT ENDLOOP;
FOR j ← y, j.join DO IF j.join = NIL THEN EXIT ENDLOOP;
IF i # j
THEN {
IF j.weight > i.weight
THEN {j.weight ← j.weight+i.weight; i.join ← z ← j}
ELSE {i.weight ← i.weight+j.weight; j.join ← z ← i};
}
ELSE z ← i;
UNTIL x.join = NIL DO t ← x; x ← x.join; t.join ← z ENDLOOP;
UNTIL y.join = NIL DO t ← y; y ← y.join; t.join ← z ENDLOOP;
};
CoupleComponents:
PUBLIC
PROC [compList: ComponentList, extraEqns: EqnList]
RETURNS [coupledList: ComponentList]
From list of graph bi-connected components (with cycle equations), and list of constraint equations, create list of coupled components. A coupled component will contain the edges and equations of all components whose edges co-occur as variables in some constraint equation, and will also contain the relevant constraint equations.
~ {
FOR eqns: EqnList ← extraEqns, eqns.rest
UNTIL eqns =
NIL
DO
terms: TermList ← eqns.first;
comp: Component ← TangleOf[terms.first.eData.join];
FOR terms ← terms.rest, terms.rest
UNTIL terms =
NIL
DO
Entangle[comp, terms.first.eData.join];
ENDLOOP;
comp.extraEqns ← CONS[eqns.first, comp.extraEqns];
ENDLOOP;
FOR comps: ComponentList ← compList, comps.rest
UNTIL comps =
NIL
DO
comp: Component ← comps.first;
tangle: Component ← TangleOf[comp];
IF tangle = comp
THEN coupledList ← CONS[comp, coupledList]
ELSE {
IF comp.edges #
NIL
THEN {
edges: EdgeList ← comp.edges;
UNTIL edges.rest = NIL DO edges ← edges.rest ENDLOOP;
edges.rest ← tangle.edges;
tangle.edges ← comp.edges;
};
IF comp.cycleEqns #
NIL
THEN {
eqnList: EqnList ← comp.cycleEqns;
UNTIL eqnList.rest = NIL DO eqnList ← eqnList.rest ENDLOOP;
eqnList.rest ← tangle.cycleEqns;
tangle.cycleEqns ← comp.cycleEqns;
};
IF comp.extraEqns #
NIL
THEN {
eqnList: EqnList ← comp.extraEqns;
UNTIL eqnList.rest = NIL DO eqnList ← eqnList.rest ENDLOOP;
eqnList.rest ← tangle.extraEqns;
tangle.extraEqns ← comp.extraEqns;
};
};
ENDLOOP;
};
NumberEdges:
PROC [edges: EdgeList]
RETURNS [nVars:
NAT ← 0]
Leave variable 0 for kludging initial feasible point.
~ {
UNTIL edges =
NIL
DO
edges.first.eData.var ← nVars ← nVars+1;
edges ← edges.rest;
ENDLOOP;
RETURN[nVars+1];
};
CountEquations:
PROC [eqnList: EqnList]
RETURNS [nEqns:
NAT ← 0]
Count number of equations in list.
~ {UNTIL eqnList = NIL DO nEqns ← nEqns+1; eqnList ← eqnList.rest ENDLOOP};
SolveComponent:
PUBLIC
PROC [component: Component]
Transform component data into QP form and call QPSolve.
~ {
penalty: REAL ← 1.0e6;
nVars: NAT ← NumberEdges[component.edges];
nEqns: NAT ← CountEquations[component.cycleEqns]+CountEquations[component.extraEqns];
lobd: RVector ← NEW[RVectorRep[nVars]];
x: RVector ← NEW[RVectorRep[nVars]];
c: RVector ← NEW[RVectorRep[nVars]];
A: Matrix ← NewMatrix[nEqns, nVars];
rowOf: IVector ← FindFreeVars[component.cycleEqns, component.extraEqns, nEqns, nVars];
iVar: IVector ← NEW[IVectorRep[nVars]];
hiMark: INT ← nVars;
nFR: NAT ← 0;
eqnList: EqnList ← component.cycleEqns;
Establish proper vector lengths.
lobd.n ← x.n ← c.n ← iVar.n ← nVars;
Fill in matrix entries from equation lists.
FOR j:
NAT
IN [0..nEqns)
DO
FOR i: NAT IN [0..nVars) DO A[j][i] ← 0.0 ENDLOOP;
FOR termList: TermList ← eqnList.first, termList.rest
UNTIL termList =
NIL
DO
i: NAT ← termList.first.eData.var;
A[j][i] ← termList.first.coef / termList.first.eData.squeeze;
ENDLOOP;
eqnList ← eqnList.rest;
IF eqnList = NIL THEN eqnList ← component.extraEqns;
ENDLOOP;
Fill in initial variable values from spread plus a little for free variables, exactly for fixed.
FOR edgeList: EdgeList ← component.edges, edgeList.rest
UNTIL edgeList =
NIL
DO
eData: EdgeData ← edgeList.first.eData;
var: NAT ← eData.var;
lobd[var] ← eData.spread*eData.squeeze;
c[var] ← -eData.relaxed/eData.squeeze;
IF rowOf[var] = noRow
THEN {iVar[hiMark ← hiMark-1] ← var; x[var] ← lobd[var]}
ELSE {iVar[(nFR ← nFR+1)] ← var; x[var] ← lobd[var]+1.0};
ENDLOOP;
Insert extra variable to offset error in equations at initial x.
iVar[0] ← 0; nFR ← nFR+1;
x[0] ← 0.0;
FOR j:
NAT
IN [0..nEqns)
DO
FOR i: NAT IN [1..nVars) DO A[j][0] ← A[j][0] + A[j][i]*x[i] ENDLOOP;
x[0] ← x[0] + A[j][0] * A[j][0];
ENDLOOP;
x[0] ← RealFns.SqRt[x[0]];
FOR j: NAT IN [0..nEqns) DO A[j][0] ← -A[j][0] / x[0] ENDLOOP;
lobd[0] ← 0.0; c[0] ← penalty;
Solve quadratic programming problem.
nFR ← QPSolve[c, A, lobd, x, iVar, nFR];
FOR edgeList: EdgeList ← component.edges, edgeList.rest
UNTIL edgeList =
NIL
DO
edgeList.first.eData.val ← x[edgeList.first.eData.var];
ENDLOOP;
};
SetValues:
PROC [v: Vertex]
~ {
v.visited ← 0;
FOR edges: EdgeList ← v.adj, edges.rest
UNTIL edges =
NIL
DO
edge: Edge ← edges.first;
w: Vertex ← edge.thatEnd;
IF w.visited # 0
THEN {w.val ← v.val+edge.dir*edge.eData.val/edge.eData.squeeze; SetValues[w]};
ENDLOOP;
};
SolveGraph:
PUBLIC
PROC [g: Graph]
Transform graph into QP form, call QPSolve, propagate positions.
~ {
biconnList: ComponentList ← FindComponents[g];
coupledList: ComponentList ← CoupleComponents[biconnList, g.eqnList];
FOR cL: ComponentList ← coupledList, cL.rest
UNTIL cL =
NIL
DO
edge: Edge ← cL.first.edges.first;
IF cL.first.edges.rest =
NIL
AND cL.first.cycleEqns =
NIL
AND cL.first.extraEqns =
NIL
THEN edge.eData.val ← edge.eData.spread*edge.eData.squeeze
ELSE SolveComponent[cL.first];
ENDLOOP;
FOR vL: VertexList ← g.vList.rest, vL.rest
UNTIL vL.first =
NIL
DO
IF vL.first.visited # 0 THEN {vL.first.val ← 0.0; SetValues[vL.first]};
ENDLOOP;
};
mem: PBasics.Word ← 12422627672B;
RandomBit:
PROC
RETURNS [on:
BOOL]
~ {
hiBit: PBasics.Word ~ 20000000000B;
bits: PBasics.Word ~ 10000000123B+hiBit;
mem ← mem+mem; -- or use PBasics.BITLSHIFT[mem, 1]
on ← mem >= hiBit; -- or use PBasics.BITAND[mem, hiBit] # 0
IF on THEN mem ← PBasics.BITXOR[mem, bits];
};
maxLevel: NAT ~ 4;
RandomLevel:
PROC
RETURNS [level: Level ← 1]
~ {
WHILE RandomBit[] AND RandomBit[] AND level < maxLevel DO level ← level + 1 ENDLOOP;
};
sentinel: VertexList ~ NEW[VertexListRep ← [name: "\377", first: NIL, forward: NIL]];
NewVertexList:
PROC [level: Level, name:
ROPE, vid:
INT ← 0]
RETURNS [vL: VertexList]
~ {
vL ← NEW[VertexListRep ← [name: name, first: NIL, forward: NEW[SkipsRep[level]]]];
IF vid = 0
THEN {vL.rest ← sentinel; FOR i: Level IN [0 .. level) DO vL.forward[i] ← sentinel ENDLOOP}
ELSE vL.first ← NEW[VertexRep ← [name: name, visited: 0, tree: NIL, adj: NIL, val: 0.0, vid: vid]];
};
NewGraph:
PUBLIC
PROC
RETURNS [g: Graph]
~ {
g ← NEW[GraphRep ← [order: 0, nV: 0, vList: NewVertexList[maxLevel, NIL, 0], eqnList: NIL]];
};
update: Skips ← NEW[SkipsRep[maxLevel]];
AcquireVertex:
PUBLIC
PROC [g: Graph, name:
ROPE]
RETURNS [v: Vertex]
~ {
list: VertexList ← g.vList;
x: VertexList ← list;
FOR i: Level
DECREASING
IN [0 .. list.forward.level)
DO
WHILE Rope.Compare[x.forward[i].name, name] = less DO x ← x.forward[i] ENDLOOP;
update[i] ← x;
ENDLOOP;
x ← x.rest;
IF
NOT Rope.Equal[x.name, name]
THEN
{
level: Level ← RandomLevel[];
x ← NewVertexList[level, name, g.nV ← g.nV+1];
x.rest ← update[0].rest; update[0].rest ← x;
FOR i: Level
IN [0 .. level)
DO
x.forward[i] ← update[i].forward[i];
update[i].forward[i] ← x;
ENDLOOP;
};
RETURN[x.first];
};
AcquireEdge:
PUBLIC
PROC [g: Graph, thisEnd, thatEnd: Vertex]
RETURNS [edge: Edge]
~ {
back: Edge;
FOR edgeList: EdgeList ← thisEnd.adj, edgeList.rest
UNTIL edgeList =
NIL
DO
IF edgeList.first.thatEnd = thatEnd THEN RETURN[edgeList.first];
ENDLOOP;
edge ← NEW[EdgeRep ← [thisEnd: thisEnd, thatEnd: thatEnd, eData: NIL, dir: 1.0]];
edge.eData ← NEW[EdgeDataRep];
thisEnd.adj ← CONS[edge, thisEnd.adj];
back ← NEW[EdgeRep ← [thisEnd: thatEnd, thatEnd: thisEnd, eData: edge.eData, dir: -1.0]];
thatEnd.adj ← CONS[back, thatEnd.adj];
};
SpreadEdge:
PUBLIC
PROC [g: Graph, edge: Edge, spread:
REAL]
~ {edge.eData.spread ← spread};
SqueezeEdge:
PUBLIC
PROC [g: Graph, edge: Edge, squeeze:
REAL]
~ {edge.eData.squeeze ← RealFns.SqRt[ABS[squeeze]]};
RelaxEdge:
PUBLIC
PROC [g: Graph, edge: Edge, relaxed:
REAL]
~ {edge.eData.relaxed ← relaxed};
ConstrainEdges:
PUBLIC
PROC [g: Graph, termList: TermList]
~ {g.eqnList ← CONS[termList, g.eqnList]};
ListGraph:
PUBLIC
PROC [g: Graph, full:
BOOL ←
FALSE]
RETURNS [list:
LIST
OF
REF ←
NIL]
~ {
FOR vL: VertexList ← g.vList.rest, vL.rest
UNTIL vL.first =
NIL
DO
v: Vertex ← vL.first;
adj: LIST OF REF ← NIL;
FOR vAdj: EdgeList ← v.adj, vAdj.rest
UNTIL vAdj =
NIL
DO
IF vAdj.first.dir > 0
THEN adj ← CONS[LIST[vAdj.first.thatEnd.name, vAdj.first.eData], adj]
ELSE {
w: Vertex ← vAdj.first.thatEnd;
FOR wAdj: EdgeList ← w.adj, wAdj.rest
UNTIL wAdj =
NIL
DO
IF wAdj.first.thatEnd = v THEN EXIT;
REPEAT
FINISHED => adj ← CONS[LIST[Rope.Concat["?", w.name], vAdj.first.eData], adj];
ENDLOOP;
};
ENDLOOP;
list ← CONS[LIST[vL.name, adj], list];
ENDLOOP;
};
ESee:
PUBLIC
PROC [edge: Edge, full:
BOOL ←
FALSE]
RETURNS [list:
LIST
OF
REF ←
NIL]
~ {
rope: ROPE;
IF edge = NIL THEN RETURN[LIST["<>"]];
rope ← Rope.Cat[edge.thisEnd.name, IF edge.dir > 0 THEN ">" ELSE "<", edge.thatEnd.name];
IF full THEN list ← LIST[rope, edge.eData] ELSE list ← LIST[rope];
};
ELSee:
PUBLIC
PROC [edges: EdgeList, full:
BOOL ←
FALSE]
RETURNS [list:
LIST
OF
REF ←
NIL]
~ {
UNTIL edges = NIL DO list ← CONS[ESee[edges.first, full], list]; edges ← edges.rest ENDLOOP;
};
ListComp:
PUBLIC
PROC [comp: Component, full:
BOOL ←
FALSE]
RETURNS [list:
LIST
OF
REF]
~ {
list ← LIST[ELSee[comp.edges, full], comp.cycleEqns, comp.extraEqns];
};
END..
FindFreeVars: PUBLIC PROC [eqnList: EqnList, nEqns, nVars: NAT]
RETURNS [rowOf: IVector]
Return rowOf filled with index of row for free vars, noRow for rest of vars.
~ {
matrix: SparseMatrix ← NEW[SparseMatrixRep[nEqns]];
visited: IVector ← NEW[IVectorRep[nEqns]];
Search: PROC [row: INT, true: INT] RETURNS [BOOL]
~ {
visited[row] ← true; -- monotonically increasing value of TRUE
FOR terms: TermList ← matrix[row], terms.rest WHILE terms # NIL DO
var: NAT ← terms.first.eData.var;
IF rowOf[var] = noRow
THEN {rowOf[var] ← row; RETURN[TRUE]};
ENDLOOP;
FOR terms: TermList ← matrix[row], terms.rest WHILE terms # NIL DO
var: NAT ← terms.first.eData.var;
nextRow: INT ← rowOf[var];
IF nextRow # row AND visited[nextRow] # true AND Search[nextRow, true]
THEN {rowOf[var] ← row; RETURN[TRUE]};
ENDLOOP;
RETURN[FALSE];
};
rowOf ← NEW[IVectorRep[nVars]];
FOR row: NAT IN [0..nEqns) DO matrix[row] ← eqnList.first; eqnList ← eqnList.rest ENDLOOP;
FOR col: NAT IN [0..nVars) DO rowOf[col] ← noRow ENDLOOP;
FOR row: NAT IN [0..nEqns) DO visited[row] ← noRow ENDLOOP;
FOR row: NAT IN [0..nEqns) DO [] ← Search[row, row] ENDLOOP;
};
FindCycles: PROC [g: Graph, treeEdge: Edge]
~ {
from: Vertex ← treeEdge.thisEnd;
v: Vertex ← treeEdge.thatEnd;
v.tree ← treeEdge;
v.visited ← g.order ← g.order+1;
FOR edges: EdgeList ← v.adj, edges.rest UNTIL edges = NIL DO
edge: Edge ← edges.first;
w: Vertex ← edge.thatEnd;
IF w.visited = 0
THEN FindCycles[g, edge]
ELSE {IF w # from THEN FormEquation[edge]};
ENDLOOP;
};
FOR vL: VertexList ← g.vList.rest, vL.rest UNTIL vL.first = NIL DO vL.first.visited ← 0 ENDLOOP;
g.order ← 0;
FOR vL: VertexList ← g.vList.rest, vL.rest UNTIL vL.first = NIL DO
IF vL.first.visited = 0 THEN [] ← FindCycles[g, NEW[EdgeRep ← [NIL, vL.first, NIL, 1.0]]];
ENDLOOP;
GetMatProc: TYPE ~ PROC [INT, INT] RETURNS [REAL];
SparseMatrixFromArray: PUBLIC PROC [getA: GetMatProc, rows, cols: NAT]
RETURNS [matrix: SparseMatrix]
~ {
matrix ← NEW[SparseMatrixRep[rows]];
FOR row: NAT IN [0..rows) DO
terms: TermList ← NIL;
FOR col: NAT IN [0..cols) DO
a: REAL ← getA[row, col];
IF a # 0.0 THEN terms ← CONS[[eData: col, coef: a], terms]
ENDLOOP;
matrix[row] ← terms;
ENDLOOP;
};