LSPieceImpl.mesa
Copyright Ó 1985, 1992 by Xerox Corporation. All rights reserved.
Wyatt, September 5, 1985 1:30:37 pm PDT
Stone, October 2, 1987 4:33:48 pm PDT
Plass, June 29, 1992 2:26 pm PDT
Bier, February 6, 1989 5:44:05 pm PST
DIRECTORY
CedarProcess, Complex, Cubic2, CubicPaths, Lines2d, LSPiece, Real, RealFns, Seq, Vector, Vectors2d;
LSPieceImpl: CEDAR PROGRAM
IMPORTS CedarProcess, Complex, Cubic2, CubicPaths, Lines2d, Real, RealFns, Vector, Vectors2d
EXPORTS LSPiece
= BEGIN
ComplexSequence: TYPE = Seq.ComplexSequence;
Metrics: TYPE = LSPiece.Metrics;
RealSequence: TYPE = Seq.RealSequence;
SingularSystem: SIGNAL = CODE;
useAve: BOOLEAN ¬ FALSE;
errInc: BOOLEAN ¬ TRUE; --make the error exit on increasing error conditional
onlyOnMaxItr: BOOLEAN ¬ FALSE; --an extreme case, for instrumentation
atLeastOnce: BOOLEAN ¬ TRUE; --initial approximation is often bad
FitPiece:
PUBLIC
PROC
[z: ComplexSequence, t: RealSequence,
metrics: LSPiece.Metrics,
from, thru: NAT,
initFree, finalFree: BOOLEAN ¬ FALSE,
initTangent, finalTangent: Complex.VEC ¬ [0,0],
useOldTValues: BOOLEAN ¬ FALSE]
RETURNS [b: Cubic2.Bezier ¬ [[0,0],[0,0],[0,0],[0,0]], err: REAL ¬ 0, iterations: INT ¬ 0, maxDev: REAL ¬ 0, indexOfMaxDev: NAT ¬ 0] = TRUSTED {
f: Cubic2.Coeffs ¬ [[0,0], [0,0], [0,0], [0,0]]; -- the current approximation
l: NAT ¬ z.length;
nSamples: NAT ¬ (IF from>thru THEN from-thru ELSE z.length-from+thru)+1;
Wrap: PROC [i: NAT] RETURNS [NAT] = TRUSTED INLINE {RETURN[IF i>=l THEN i-l ELSE i]};
ArcLengthInitialTValues:
PROC =
TRUSTED {
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:
PROC
RETURNS [s:
REAL] =
TRUSTED {
OPEN f;
maxDevSqr: REAL ¬ 0;
s ¬ 0;
indexOfMaxDev ¬ from;
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; indexOfMaxDev ¬ i};
IF i=thru THEN EXIT
ENDLOOP;
maxDev ¬ RealFns.SqRt[maxDevSqr];
IF useAve THEN s ¬ s/nSamples;
};
Adjustment:
PROC [z: Complex.
VEC, t:
REAL]
RETURNS [delta:REAL] = TRUSTED 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:
PROC
RETURNS [maxChange:
REAL] =
TRUSTED {
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:
PROC =
TRUSTED 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 Cubic2.Coeffs;
free: ARRAY [0..8) OF BOOLEAN;
a: ARRAY [0..8) OF REAL;
nFree: NAT ¬ 0;
SetupBasis:
PROC =
TRUSTED {
FOR i:NAT IN [0..8) DO free[i] ¬ FALSE ENDLOOP;
basis[0] ¬ Cubic2.BezierToCoeffs[[[1,0],[1,0],[0,0],[0,0]]]; a[0] ¬ z[from].x;
basis[1] ¬ Cubic2.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] ¬ Cubic2.BezierToCoeffs[[[0,0],[1,0],[0,0],[0,0]]]; free[2] ¬ TRUE;
basis[3] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,1],[0,0],[0,0]]]; free[3] ¬ TRUE;
}
ELSE {
basis[2] ¬ Cubic2.BezierToCoeffs[[[0,0],initTangent,[0,0],[0,0]]]; free[2] ¬ TRUE;
basis[3] ¬ Cubic2.BezierToCoeffs[[[0,0],Complex.Mul[initTangent,[0,1]],[0,0],[0,0]]]; a[3] ¬ 0;
};
IF finalTangent=[0,0]
THEN {
basis[4] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[0,0]]]; free[4] ¬ TRUE;
basis[5] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,0]]]; free[5] ¬ TRUE;
}
ELSE {
basis[4] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],finalTangent,[0,0]]]; free[4] ¬ TRUE;
basis[5] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],Complex.Mul[finalTangent,[0,1]],[0,0]]]; a[5] ¬ 0;
};
basis[6] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[1,0]]]; a[6] ¬ z[thru].x;
basis[7] ¬ Cubic2.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:
PROC [i:
NAT, t:
REAL]
RETURNS [Complex.
VEC] =
TRUSTED {
b: Cubic2.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:
PROC[n:
NAT] =
TRUSTED {
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:
PROC =
TRUSTED {
singular: BOOLEAN ¬ FALSE;
SetupSystem:
PROC =
TRUSTED {
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;
};
SetupSystem[ ! Real.RealException => CHECKED {singular ¬ TRUE; CONTINUE}];
IF
NOT singular
THEN 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:
PROC =
CHECKED {
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..metrics.maxItr)
DO
IF initFree OR finalFree THEN Rescale;
SolveCurve;
CedarProcess.CheckAbort[]; --once per iteration
FindCoeffs;
err ¬ Error[];
make the error exit on increasing error conditional
IF
NOT onlyOnMaxItr
AND (atLeastOnce
AND iterations > 0)
AND
((errInc AND err>olderr) OR err<metrics.sumErr OR maxDev<metrics.maxDev) THEN EXIT;
olderr ¬ err;
IF AdjustTValues[]
NOT
IN [metrics.deltaT..50]
AND
NOT onlyOnMaxItr
THEN
EXIT;
Bail out early if the t values change radically
ENDLOOP;
b ¬ Cubic2.CoeffsToBezier[f];
};
FitPiece2:
PUBLIC
PROC
[z: ComplexSequence, t: RealSequence,
metrics: Metrics,
from, thru: NAT,
initFree, finalFree: BOOLEAN ¬ FALSE,
initTangent, finalTangent: Complex.VEC ¬ [0,0],
useOldTValues: BOOLEAN ¬ FALSE,
someInterp: BOOLEAN ¬ FALSE,
realSample: LSPiece.RealSample ¬ NIL]
RETURNS [b: Cubic2.Bezier ¬ [[0,0],[0,0],[0,0],[0,0]], err: REAL ¬ 0, iterations: INT ¬ 0, maxDev: REAL ¬ 0, indexOfMaxDev: NAT ¬ 0] = TRUSTED {
f: Cubic2.Coeffs ¬ [[0,0],[0,0],[0,0],[0,0]]; -- the current approximation
l: NAT ¬ z.length;
nSamples: NAT ¬ (IF from>thru THEN from-thru ELSE z.length-from+thru)+1;
Wrap: PROC [i: NAT] RETURNS [NAT] = CHECKED INLINE {RETURN[IF i>=l THEN i-l ELSE i]};
ArcLengthInitialTValues:
PROC =
CHECKED {
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:
PROC
RETURNS [s:
REAL] =
CHECKED {
OPEN f;
maxDevSqr: REAL ¬ 0;
s ¬ 0;
indexOfMaxDev ¬ from;
FOR i:
NAT ¬ from, Wrap[i+1]
DO
IF
NOT someInterp
OR realSample[i]
THEN {
-- if not a realSample then error does not matter.
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; indexOfMaxDev ¬ i};
};
IF i=thru THEN EXIT
ENDLOOP;
maxDev ¬ RealFns.SqRt[maxDevSqr];
IF useAve THEN s ¬ s/nSamples; -- nSamples may be wrong if interp.
};
Adjustment:
PROC [z: Complex.
VEC, t:
REAL]
RETURNS [delta:REAL] = CHECKED 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:
PROC
RETURNS [maxChange:
REAL] =
CHECKED {
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:
PROC =
CHECKED 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 Cubic2.Coeffs;
free: ARRAY [0..8) OF BOOLEAN;
a: ARRAY [0..8) OF REAL;
nFree: NAT ¬ 0;
SetupBasis:
PROC =
CHECKED {
FOR i:NAT IN [0..8) DO free[i] ¬ FALSE ENDLOOP;
basis[0] ¬ Cubic2.BezierToCoeffs[[[1,0],[1,0],[0,0],[0,0]]]; a[0] ¬ z[from].x;
basis[1] ¬ Cubic2.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] ¬ Cubic2.BezierToCoeffs[[[0,0],[1,0],[0,0],[0,0]]]; free[2] ¬ TRUE;
basis[3] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,1],[0,0],[0,0]]]; free[3] ¬ TRUE;
}
ELSE {
basis[2] ¬ Cubic2.BezierToCoeffs[[[0,0],initTangent,[0,0],[0,0]]]; free[2] ¬ TRUE;
basis[3] ¬ Cubic2.BezierToCoeffs[[[0,0],Complex.Mul[initTangent,[0,1]],[0,0],[0,0]]]; a[3] ¬ 0;
};
IF finalTangent=[0,0]
THEN {
basis[4] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[0,0]]]; free[4] ¬ TRUE;
basis[5] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,0]]]; free[5] ¬ TRUE;
}
ELSE {
basis[4] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],finalTangent,[0,0]]]; free[4] ¬ TRUE;
basis[5] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],Complex.Mul[finalTangent,[0,1]],[0,0]]]; a[5] ¬ 0;
};
basis[6] ¬ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[1,0]]]; a[6] ¬ z[thru].x;
basis[7] ¬ Cubic2.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:
PROC [i:
NAT, t:
REAL]
RETURNS [Complex.
VEC] =
CHECKED {
b: Cubic2.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:
PROC[n:
NAT] =
TRUSTED {
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:
PROC =
TRUSTED {
singular: BOOLEAN ¬ FALSE;
SetupSystem:
PROC =
TRUSTED {
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;
};
SetupSystem[ ! Real.RealException => CHECKED {singular ¬ TRUE; CONTINUE}];
IF
NOT singular
THEN 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:
PROC =
CHECKED {
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..metrics.maxItr)
DO
IF initFree OR finalFree THEN Rescale;
SolveCurve;
CedarProcess.CheckAbort[]; --once per iteration
FindCoeffs;
err ¬ Error[];
make the error exit on increasing error conditional
IF
NOT onlyOnMaxItr
AND (atLeastOnce
AND iterations > 0)
AND
((errInc AND err>olderr) OR err<metrics.sumErr OR maxDev<metrics.maxDev) THEN EXIT;
olderr ¬ err;
IF AdjustTValues[]
NOT
IN [metrics.deltaT..50]
AND
NOT onlyOnMaxItr
THEN
EXIT;
Bail out early if the t values change radically
ENDLOOP;
b ¬ Cubic2.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:
PROC [
A: Matrix, b: Column, n:
NAT, x: Column] =
TRUSTED {
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;
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;
ComputeCurvature:
PUBLIC SAFE PROC [farCP, nearCP, joint: Complex.
VEC, jointFirst:
BOOL]
RETURNS [
REAL] ~ {
jointLine: Lines2d.Line;
aDistSq, hDist: REAL;
IF jointFirst THEN jointLine ¬ Lines2d.LineFromPoints[joint, nearCP]
ELSE jointLine ¬ Lines2d.LineFromPoints[nearCP, joint];
hDist ¬ Lines2d.SignedLineDistance[farCP, jointLine];
aDistSq ¬ Vectors2d.DistanceSquared[nearCP, joint];
RETURN[ 4/3.0 * hDist/aDistSq];
};
CurvatureAtPt:
PUBLIC SAFE PROC [bezier: Cubic2.Bezier, pt: Complex.
VEC]
RETURNS [
REAL] ~
TRUSTED{
tBezier: Cubic2.Bezier;
IF pt = bezier.b0 THEN RETURN[ComputeCurvature[bezier.b2, bezier.b1, bezier.b0, TRUE]];
IF pt = bezier.b3 THEN RETURN[ComputeCurvature[bezier.b1, bezier.b2, bezier.b3, FALSE]];
[tBezier, ----] ¬ Cubic2.AlphaSplit[bezier, CubicPaths.GetParam[bezier, pt]];
RETURN[ComputeCurvature[tBezier.b1, tBezier.b2, tBezier.b3, FALSE]];
};
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.