DIRECTORY
CedarProcess, Complex, Cubic2, Cubic2Extras, CubicPathsExtras, Lines2d, LSPiece, LSPieceExtras, Real, RealFns, Seq, Vector, Vectors2d;

LSPieceImpl: CEDAR PROGRAM
IMPORTS CedarProcess, Complex, Cubic2, Cubic2Extras, CubicPathsExtras, Lines2d, Real, RealFns, Vector, Vectors2d
EXPORTS LSPiece, LSPieceExtras
= 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, err: REAL, iterations: INT, maxDev: REAL, indexOfMaxDev: NAT] = TRUSTED {
f: Cubic2.Coeffs; -- 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;
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[];
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;
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: LSPieceExtras.RealSample _ NIL]
RETURNS [b: Cubic2.Bezier, err: REAL, iterations: INT, maxDev: REAL, indexOfMaxDev: NAT] = TRUSTED {
f: Cubic2.Coeffs; -- 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;
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[];
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;
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 {
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;
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, ----] _ Cubic2Extras.AlphaSplit[bezier, CubicPathsExtras.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.

LSPieceImpl.mesa
Copyright c 1985 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, October 12, 1987 5:29:39 pm PDT
Bier, February 6, 1989 5:44:05 pm PST
make the error exit on increasing error conditional
Bail out early if the t values change radically
make the error exit on increasing error conditional
Bail out early if the t values change radically
solve Ax=b by Gaussian Elimination
Now A is upper-triangular, so back substitute
Κ
˜
codeš
œ™Kšœ
Οm
œ1™<K™'K™%K™&K™%—K˜š
Οk	˜	Jš
Ϡ
˜†
—K˜KšΠblœž
˜Jšžœi˜pKšžœ˜Kšœž˜K˜Kšœžœ˜,Kšœ	žœ˜ Kšœžœ˜&Kšœžœžœ
˜Kšœžœžœ
˜KšœžœžœΟc5˜MKšœžœžœ &˜EKšœ
žœžœ $˜AK˜šΟnœžœ
ž˜K˜%Kš
œ˜Kšœžœ
˜Kšœžœžœ
˜%Kšœ#žœ	˜/Kšœžœžœ
˜Kšžœžœžœ
žœžœžœ˜dKšœ ˜.Kšœžœ˜Kš	œ
žœžœžœžœ˜HKš‘œžœžœžœžœžœžœ
žœžœžœ˜Uš‘œžœžœ˜)Kšœžœ˜K˜šžœžœžœž˜;Kšœ1žœžœžœ˜MK˜Kšžœ
˜—šžœžœžœž˜;K˜Kšžœ
˜—K˜—š
‘œžœ
žœžœžœ˜)Kšžœ˜Kšœžœ˜K˜Kš
œ˜šžœžœž˜Kšœžœ˜Kšœžœ$˜*Kšœžœ$˜*Kšœžœ˜Kšœžœ)˜/K˜Kšžœ
žœ$˜7Kšžœžœ
ž˜Kšžœ
˜—K˜!Kšžœžœ˜K˜—š‘
œžœ
žœžœ
˜*Kšžœžœžœ˜'Kšžœ˜Kšœ
žœ
˜Kšœ
žœ
˜Kšœžœ
˜Kšœžœ
˜K˜"K˜K˜K˜"K˜K˜K˜'K˜>Kšœžœžœžœ!˜KK˜—š
‘
œžœ
žœ
žœžœ˜9Kšœžœ
˜K˜šžœžœž˜K˜Kšžœ
žœžœ
žœ˜6K˜Kšžœžœ
ž˜Kšžœ
˜—K˜—š‘œžœžœ˜ Kšœžœ˜Kšœ
žœ˜šžœžœž˜K˜Kšžœžœ
ž˜Kšžœ
˜—K˜—Kšœžœžœ˜%Kšœžœžœ
žœ
˜Kšœžœžœ
žœ
˜Kšœžœ
˜š‘
œžœžœ˜Kšžœžœ
žœžœžœ
žœ
˜/Kš
œN˜NKš
œN˜NKšžœ
žœžœ
˜*šžœžœ˜KšœGžœ
˜LKšœGžœ
˜LK˜
—šžœ˜KšœMžœ
˜RKš
œ`˜`K˜—šžœžœ˜KšœGžœ
˜LKšœGžœ
˜LK˜
—šžœ˜KšœNžœ
˜SKš
œa˜aK˜—Kš
œN˜NKš
œN˜NKšžœžœžœ
˜+Kšœžœžœ
žœžœ
žœ	žœžœ
˜LK˜—š‘	œžœžœžœžœ
žœžœ˜CKš
œ˜šžœ˜K˜&K˜%K˜—K˜—K˜K˜K˜Kšœžœžœ
žœ
˜+Kšœžœžœ˜'š‘œžœžœžœ˜2Kšœžœ
žœ
˜Kšžœžœ
žœ
˜K˜Kšœž
œ˜K˜šžœžœ
žœž˜Kšœž
œ˜Kšœ
žœ
žœ˜Kšžœ
˜—Kšœž
œ˜Kšœ
žœ
žœ˜Kšœ
ž
œ˜Kšœ
žœ
žœ˜K˜—š‘
œžœžœ˜Kšœ
žœžœ
˜š‘œžœžœ˜šžœžœ
žœž˜Kš
žœžœ
žœžœžœ
˜7K˜Kšžœ
˜—šžœžœž˜Kšœžœ˜Kšœžœ˜
šžœžœ
žœžœ
žœ	žœ˜)Kšœžœ˜"Kšœžœ˜šžœžœ
žœž˜Kšœžœ˜"šžœ	ž˜K˜A—š
ž˜K˜9—Kšžœ	žœ˜$Kšžœ
˜—K˜6K˜K˜Kšžœ
˜—Kšžœžœ
ž˜Kšžœ
˜—Kš
œ˜—Kšœ%žœ
žœžœ˜Jšžœ
žœ
žœ8˜LKšœžœ
žœžœ˜;—šžœ
žœ˜Kšžœžœ
žœ˜;K˜.šžœžœ
žœž˜Kšžœ	žœ
˜Kšžœ˜	—K˜
—šžœ˜Kšœžœ˜
šžœžœ
žœž˜Kšžœ	žœ&˜5Kšžœ
˜—K˜—K˜—š‘
œžœžœ˜K˜šžœžœ
žœž˜˜4K˜/K˜/K˜0—Kšžœ
˜—K˜K˜—Kšœžœ˜Kšžœ
žœžœ˜2K˜K˜ šžœžœž˜(Kšžœ
žœžœ	˜&K˜Kšœ ˜/K˜K˜K™3š	žœ
žœžœžœž˜=Kšœ	žœ
žœžœžœ
žœ
˜S—K˜
šžœžœ
žœžœ
žœžœ
žœ
˜NK™/—Kšžœ
˜—Kš
œ˜K˜K˜—š‘	œžœ
ž˜K˜%Kš
œ˜Kšœžœ
˜Kšœžœžœ
˜%Kšœ#žœ	˜/Kšœžœžœ
˜Kšœžœžœ
˜Kšœ'žœ
˜+Kšžœžœžœ
žœžœžœ˜dKšœ ˜.Kšœžœ˜Kš	œ
žœžœžœžœ˜HKš‘œžœžœžœžœžœžœ
žœžœžœ˜Uš‘œžœžœ˜)Kšœžœ˜K˜šžœžœžœž˜;Kšœ1žœžœžœ˜MK˜Kšžœ
˜—šžœžœžœž˜;K˜Kšžœ
˜—K˜K˜—š
‘œžœ
žœžœžœ˜)Kšžœ˜Kšœžœ˜K˜Kš
œ˜šžœžœž˜š	žœ
žœžœžœ 2˜]Kšœžœ˜Kšœžœ$˜*Kšœžœ$˜*Kšœžœ˜Kšœžœ)˜/K˜Kšžœ
žœ$˜7K˜—Kšžœžœ
ž˜Kšžœ
˜—K˜!Kšžœžœ #˜BK˜—š‘
œžœ
žœžœ
˜*Kšžœžœžœ˜'Kšžœ˜Kšœ
žœ
˜Kšœ
žœ
˜Kšœžœ
˜Kšœžœ
˜K˜"K˜K˜K˜"K˜K˜K˜'K˜>Kšœžœžœžœ!˜KK˜—š
‘
œžœ
žœ
žœžœ˜9Kšœžœ
˜K˜šžœžœž˜K˜Kšžœ
žœžœ
žœ˜6K˜Kšžœžœ
ž˜Kšžœ
˜—K˜—š‘œžœžœ˜ Kšœžœ˜Kšœ
žœ˜šžœžœž˜K˜Kšžœžœ
ž˜Kšžœ
˜—K˜—Kšœžœžœ˜%Kšœžœžœ
žœ
˜Kšœžœžœ
žœ
˜Kšœžœ
˜š‘
œžœžœ˜Kšžœžœ
žœžœžœ
žœ
˜/Kš
œN˜NKš
œN˜NKšžœ
žœžœ
˜*šžœžœ˜KšœGžœ
˜LKšœGžœ
˜LK˜
—šžœ˜KšœMžœ
˜RKš
œ`˜`K˜—šžœžœ˜KšœGžœ
˜LKšœGžœ
˜LK˜
—šžœ˜KšœNžœ
˜SKš
œa˜aK˜—Kš
œN˜NKš
œN˜NKšžœžœžœ
˜+Kšœžœžœ
žœžœ
žœ	žœžœ
˜LK˜—š‘	œžœžœžœžœ
žœžœ˜CKš
œ˜šžœ˜K˜&K˜%K˜—K˜—K˜K˜K˜Kšœžœžœ
žœ
˜+Kšœžœžœ˜'š‘œžœžœžœ˜2Kšœžœ
žœ
˜Kšžœžœ
žœ
˜K˜Kšœž
œ˜K˜šžœžœ
žœž˜Kšœž
œ˜Kšœ
žœ
žœ˜Kšžœ
˜—Kšœž
œ˜Kšœ
žœ
žœ˜Kšœ
ž
œ˜Kšœ
žœ
žœ˜K˜—š‘
œžœžœ˜Kšœ
žœžœ
˜š‘œžœžœ˜šžœžœ
žœž˜Kš
žœžœ
žœžœžœ
˜7K˜Kšžœ
˜—šžœžœž˜Kšœžœ˜Kšœžœ˜
šžœžœ
žœžœ
žœ	žœ˜)Kšœžœ˜"Kšœžœ˜šžœžœ
žœž˜Kšœžœ˜"šžœ	ž˜K˜A—š
ž˜K˜9—Kšžœ	žœ˜$Kšžœ
˜—K˜6K˜K˜Kšžœ
˜—Kšžœžœ
ž˜Kšžœ
˜—Kš
œ˜—Kšœ%žœ
žœžœ˜Jšžœ
žœ
žœ8˜LKšœžœ
žœžœ˜;—šžœ
žœ˜Kšžœžœ
žœ˜;K˜.šžœžœ
žœž˜Kšžœ	žœ
˜Kšžœ˜	—K˜
—šžœ˜Kšœžœ˜
šžœžœ
žœž˜Kšžœ	žœ&˜5Kšžœ
˜—K˜—K˜—š‘
œžœžœ˜K˜šžœžœ
žœž˜˜4K˜/K˜/K˜0—Kšžœ
˜—K˜K˜—Kšœžœ˜Kšžœ
žœžœ˜2K˜K˜ šžœžœž˜(Kšžœ
žœžœ	˜&K˜Kšœ ˜/K˜K˜K™3š	žœ
žœžœžœž˜=Kšœ	žœ
žœžœžœ
žœ
˜S—K˜
šžœžœ
žœžœ
žœžœ
žœ
˜NK™/—Kšžœ
˜—Kš
œ˜K˜K˜—Kš
œžœžœ
ž
œ
žœ
žœ
žœ˜0Kšœžœžœ
ž
œ
žœ
žœ
žœ
žœ
˜.Kšœžœžœ
ž
œ
žœ
žœ
žœ
žœ
˜1K˜š
‘œžœ‘
œžœžœ˜MKš
œ"™"šžœžœ
žœž˜Kšœžœ˜šžœžœ
žœž˜Kšžœ
žœ
ž
œ
žœ
ž
œžœ˜2Kšžœ
˜—Kš	œ
ž
œž
œž
œ	ž
œ
˜/Kšœžœ(˜0šžœžœ
žœž˜šžœ
ž
œ	žœ˜Kšœžœž
œž
œ 8˜Sšžœžœ
žœž˜Kšž
œ	ž
œ	ž
œ˜Kšžœ
˜—K˜K˜—Kšžœ
˜—Kšžœ
˜—Kš
œ-™-Kš
žœžœ
žœžœžœ
˜*š	žœžœ
ž
œ
žœž˜"Kšœžœ˜šžœžœ
žœž˜Kšœ
ž
œ˜Kšžœ
˜—Kšœž
œ˜Kšžœ˜—K˜K˜—Kšœžœžœ
˜.Kšœžœ˜K˜š‘œžœ žœžœžœžœ˜kKš
œ˜Kšœžœ
˜Kšžœžœ2˜DKšžœ3˜7Kš
œ5˜5Kš
œ3˜3Kšžœ˜K˜—K˜š‘
œžœ%žœžœžœžœ
˜bKš
œ˜Kšžœžœ
žœ3žœ˜WKšžœžœ
žœ3žœ˜XKš
œY˜YKšžœ6žœ˜DK˜K˜—Kšžœ
˜K˜K˜fK˜K˜EK˜K˜YK˜—…—>pVy