[Cyan]<PreISLCedar>Fit>LSPieceImpl.mesa!2

LSPieceImpl.mesa

Maureen Stone April 29, 1983 2:37 pm took out an INLINE for spying

Michael Plass August 26, 1982 1:41 pm

Maureen Stone March 19, 1984 4:03:45 pm PST

DIRECTORY

Complex,

Cubic,

LSPiece,

Real USING [RealException],

RealFns USING[SqRt],

Seq,

Vector;

LSPieceImpl: PROGRAM

IMPORTS

Complex, Cubic, Real, Vector, RealFns

EXPORTS LSPiece =

BEGIN OPEN Seq;

SingularSystem: SIGNAL = CODE;

FitPiece: PUBLIC SAFE PROCEDURE

[z: ComplexSequence, t: RealSequence,

from, thru: NAT,

eps: REAL ← .00001,

maxd: REAL ← .00001,

maxit: NAT ← 500,

deltat: REAL ← 0.000005,

initFree, finalFree: BOOLEAN ← FALSE,

initTangent, finalTangent: Complex.Vec ← [0,0],

useOldTValues: BOOLEAN ← FALSE]

RETURNS [b: Cubic.Bezier, err: REAL, iterations: NAT, maxDev: REAL] = TRUSTED {

f: Cubic.Coeffs; -- the current approximation

l: NAT ← z.length;

Wrap: PROC [i: NAT] RETURNS [NAT] = INLINE {RETURN[IF i>=l THEN i-l ELSE i]};

ArcLengthInitialTValues: PROCEDURE = {

arcLen: REAL ← 0;

t[from] ← 0;

FOR i:NAT ← Wrap[from+1], Wrap[i+1] UNTIL i=Wrap[thru+1] DO

arcLen ← arcLen + Complex.Abs[Complex.Sub[z[i],z[IF i=0 THEN l-1 ELSE i-1]]];

t[i] ← arcLen;

ENDLOOP;

FOR i:NAT ← Wrap[from+1], Wrap[i+1] UNTIL i=Wrap[thru+1] DO

t[i] ← t[i]/arcLen

ENDLOOP;

};

Error: PROCEDURE RETURNS [s: REAL] = {

OPEN f;

maxDevSqr: REAL ← 0;

s ← 0;

FOR i:NAT ← from, Wrap[i+1] DO

ti:REAL ← t[i];

x:REAL ← ((c3.x*ti+c2.x)*ti+c1.x)*ti+c0.x;

y:REAL ← ((c3.y*ti+c2.y)*ti+c1.y)*ti+c0.y;

zi: Complex.Vec ← z[i];

d:REAL ← (zi.x-x)*(zi.x-x) + (zi.y-y)*(zi.y-y);

s ← s+d;

IF d>maxDevSqr THEN maxDevSqr ← d;

IF i=thru THEN EXIT

ENDLOOP;

maxDev ← RealFns.SqRt[maxDevSqr];

};

Adjustment: PROCEDURE [z: Complex.Vec, t: REAL]

RETURNS [delta:REAL] = INLINE {

OPEN f;

x,dx,ddx: REAL;

y,dy,ddy: REAL;

derivSqrDist: REAL;

derivDerivSqrDist: REAL;

x ← ((c3.x*t+c2.x)*t+c1.x)*t+c0.x;

dx ← (3*c3.x*t+2*c2.x)*t+c1.x;

ddx ← 6*c3.x*t+2*c2.x;

y ← ((c3.y*t+c2.y)*t+c1.y)*t+c0.y;

dy ← (3*c3.y*t+2*c2.y)*t+c1.y;

ddy ← 6*c3.y*t+2*c2.y;

derivSqrDist ← (x-z.x)*dx + (y-z.y)*dy;

derivDerivSqrDist ← dx*dx + dy*dy + (x-z.x)*ddx + (y-z.y)*ddy;

delta ← IF derivDerivSqrDist=0 THEN 0 ELSE -derivSqrDist/derivDerivSqrDist;

};

AdjustTValues: PROCEDURE RETURNS [maxChange: REAL] = {

delta: REAL;

maxChange ← 0;

FOR i:NAT ← from, Wrap[i+1] DO

delta ← Adjustment[z[i],t[i]];

IF ABS[delta] > maxChange THEN maxChange ← ABS[delta];

t[i] ← t[i]+delta;

IF i=thru THEN EXIT

ENDLOOP;

};

Rescale: PROCEDURE = INLINE {

t0: REAL ← t[from];

interval: REAL ← t[thru]-t0;

FOR i:NAT ← from, Wrap[i+1] DO

t[i] ← (t[i]-t0)/interval;

IF i=thru THEN EXIT

ENDLOOP;

};

basis: ARRAY [0..8) OF Cubic.Coeffs;

free: ARRAY [0..8) OF BOOLEAN;

a: ARRAY [0..8) OF REAL;

nFree: NAT;

SetupBasis: PROCEDURE = {

FOR i:NAT IN [0..8) DO free[i] ← FALSE ENDLOOP;

basis[0] ← Cubic.BezierToCoeffs[[[1,0],[1,0],[0,0],[0,0]]]; a[0] ← z[from].x;

basis[1] ← Cubic.BezierToCoeffs[[[0,1],[0,1],[0,0],[0,0]]]; a[1] ← z[from].y;

IF initFree THEN free[0] ← free[1] ← TRUE;

IF initTangent=[0,0] THEN {

basis[2] ← Cubic.BezierToCoeffs[[[0,0],[1,0],[0,0],[0,0]]]; free[2] ← TRUE;

basis[3] ← Cubic.BezierToCoeffs[[[0,0],[0,1],[0,0],[0,0]]]; free[3] ← TRUE;

}

ELSE {

basis[2] ← Cubic.BezierToCoeffs[[[0,0],initTangent,[0,0],[0,0]]]; free[2] ← TRUE;

basis[3] ← Cubic.BezierToCoeffs[[[0,0],Complex.Mul[initTangent,[0,1]],[0,0],[0,0]]]; a[3] ← 0;

};

IF finalTangent=[0,0] THEN {

basis[4] ← Cubic.BezierToCoeffs[[[0,0],[0,0],[1,0],[0,0]]]; free[4] ← TRUE;

basis[5] ← Cubic.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,0]]]; free[5] ← TRUE;

}

ELSE {

basis[4] ← Cubic.BezierToCoeffs[[[0,0],[0,0],finalTangent,[0,0]]]; free[4] ← TRUE;

basis[5] ← Cubic.BezierToCoeffs[[[0,0],[0,0],Complex.Mul[finalTangent,[0,1]],[0,0]]]; a[5] ← 0;

};

basis[6] ← Cubic.BezierToCoeffs[[[0,0],[0,0],[1,0],[1,0]]]; a[6] ← z[thru].x;

basis[7] ← Cubic.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,1]]]; a[7] ← z[thru].y;

IF finalFree THEN free[6] ← free[7] ← TRUE;

nFree ← 0; FOR i:NAT IN [0..8) DO IF free[i] THEN nFree ← nFree + 1 ENDLOOP;

};

EvalBasis: PROCEDURE [i: NAT, t: REAL] RETURNS [Complex.Vec] = {

b: Cubic.Coeffs ← basis[i];

RETURN[[

((b.c3.x*t+b.c2.x)*t+b.c1.x)*t+b.c0.x,

((b.c3.y*t+b.c2.y)*t+b.c1.y)*t+b.c0.y

]]

};

rightSide: Column;

leftSide: Matrix;

unknown: Column;

arrayStorage: ARRAY [0..8*(8+1+1)) OF REAL;

descriptorStorage: ARRAY [0..8) OF Row;

AllocateArrayDescriptors: PROCEDURE[n: NAT] = {

p: LONG POINTER;

IF n>8 THEN ERROR;

p ← @descriptorStorage;

leftSide ← DESCRIPTOR[p, n];

p ← @arrayStorage;

FOR i: NAT IN [0..n) DO

leftSide[i] ← DESCRIPTOR[p, n];

p ← p + n*SIZE[REAL];

ENDLOOP;

rightSide ← DESCRIPTOR[p, n];

p ← p + n*SIZE[REAL];

unknown ← DESCRIPTOR[p, n];

p ← p + n*SIZE[REAL];

};

SolveCurve: PROCEDURE = {

singular: BOOLEAN ← FALSE;

FOR i: NAT IN [0..nFree) DO

FOR j: NAT IN [0..nFree) DO leftSide[i][j] ← 0 ENDLOOP;

rightSide[i] ← 0;

ENDLOOP;

FOR k: NAT ← from, Wrap[k+1] DO

tk: REAL = t[k];

row: NAT ← 0;

FOR i: NAT IN [0..8) DO IF free[i] THEN {

gi: Complex.Vec ← EvalBasis[i,tk];

column: NAT ← 0;

FOR j: NAT IN [0..8) DO

gj: Complex.Vec ← EvalBasis[j,tk];

IF free[j] THEN

leftSide[row][column] ← leftSide[row][column] + Vector.Dot[gi,gj]

ELSE

rightSide[row] ← rightSide[row] - a[j]*Vector.Dot[gi,gj];

IF free[j] THEN column ← column + 1;

ENDLOOP;

rightSide[row] ← rightSide[row] + Vector.Dot[z[k],gi];

row ← row + 1;

};

ENDLOOP;

IF k=thru THEN EXIT

ENDLOOP;

SolveLinearSystem[leftSide, rightSide, nFree, unknown!

Real.RealException => CHECKED {singular ← TRUE; CONTINUE}];

IF singular THEN {

IF paranoidAboutSingularSystems THEN SIGNAL SingularSystem;

singularSystemCount ← singularSystemCount + 1;

FOR i:NAT IN [0..8) DO

IF free[i] THEN a[i] ← 0;

ENDLOOP;

}

ELSE {

row: NAT ← 0;

FOR i:NAT IN [0..8) DO

IF free[i] THEN {a[i] ← unknown[row]; row ← row + 1};

ENDLOOP;

};

FindCoeffs: PROCEDURE = {

f ← [[0,0],[0,0],[0,0],[0,0]];

FOR i:NAT IN [0..8) DO

f ← [Complex.Add[f.c0,Vector.Mul[basis[i].c0,a[i]]],

Complex.Add[f.c1,Vector.Mul[basis[i].c1,a[i]]],

Complex.Add[f.c2,Vector.Mul[basis[i].c2,a[i]]],

Complex.Add[f.c3,Vector.Mul[basis[i].c3,a[i]]]];

ENDLOOP;

};

olderr: REAL ← 1.0E+20;

IF NOT useOldTValues THEN ArcLengthInitialTValues;

SetupBasis;

AllocateArrayDescriptors[nFree];

FOR iterations IN [0..maxit) DO

IF initFree OR finalFree THEN Rescale;

SolveCurve;

FindCoeffs;

err ← Error[];

IF err>olderr OR err<eps OR maxDev<maxd THEN EXIT;

olderr ← err;

IF AdjustTValues[]<deltat THEN EXIT;

ENDLOOP;

b ← Cubic.CoeffsToBezier[f];

};

Matrix: TYPE = LONG DESCRIPTOR FOR ARRAY OF Row;

Row: TYPE = LONG DESCRIPTOR FOR ARRAY OF REAL;

Column: TYPE = LONG DESCRIPTOR FOR ARRAY OF REAL;

SolveLinearSystem: PROCEDURE [A: Matrix, b: Column, n: NAT, x: Column] = {

solve Ax=b by Gaussian Elimination

FOR i: NAT IN [0..n) DO

bestk: NAT ← i;

FOR k: NAT IN [i..n) DO

IF ABS[A[k][i]] > ABS[A[bestk][i]] THEN bestk ← k;

ENDLOOP;

{t: Row ← A[i]; A[i] ← A[bestk]; A[bestk] ← t};

{t: REAL ← b[i]; b[i] ← b[bestk]; b[bestk] ← t};

FOR k: NAT IN (i..n) DO

IF A[k][i]#0 THEN {

r: REAL = A[k][i]/A[i][i]; -- Singular A causes Real.RealException = divide by zero

FOR j: NAT IN [i..n) DO

A[k][j] ← A[k][j] - A[i][j]*r

ENDLOOP;

b[k] ← b[k] - b[i]*r

};

ENDLOOP;

Now A is upper-triangular, so back substitute

FOR i: NAT IN [0..n) DO x[i] ← 0; ENDLOOP;

FOR i: NAT DECREASING IN [0..n) DO

xi: REAL ← b[i];

FOR j: NAT IN (i..n) DO

xi ← xi - A[i][j]*x[j];

ENDLOOP;

x[i] ← xi / A[i][i];

ENDLOOP;

};

paranoidAboutSingularSystems: BOOLEAN ← FALSE;

singularSystemCount: INT ← 0;

END.

Michael Plass August 18, 1982 10:03 am: Switched to array descriptors to eliminate excess allocations

Michael Plass August 19, 1982 1:17 pm: Cleaned up error calculation.

Michael Plass August 26, 1982 1:41 pm: Allowed the SingularSystem signal to be disabled.