-- LSFitImpl.mesa
-- Last edited by Michael Plass 10-Mar-82 15:23:07
-- Michael Plass and Maureen Stone Oct-81
DIRECTORY
Complex,
LinearSystem,
Real USING [RealException],
LSFit;
LSFitImpl: PROGRAM IMPORTS Complex, LinearSystem, Real EXPORTS LSFit =
BEGIN
RealProc: TYPE = PROCEDURE[i:NAT] RETURNS [REAL];
ComplexSequence: TYPE = LSFit.ComplexSequence;
ComplexSequenceRec: TYPE = LSFit.ComplexSequenceRec;
RealSequence: TYPE = LSFit.RealSequence;
RealSequenceRec: TYPE = LSFit.RealSequenceRec;
PointNumber: TYPE = NAT;
Patch: TYPE = LSFit.Patch;
PatchSequence: TYPE = LSFit.PatchSequence;
PatchSequenceRec: TYPE = LSFit.PatchSequenceRec;
Handle: TYPE = LSFit.Handle;
StateRec: TYPE = LSFit.StateRec;
Create: PUBLIC PROCEDURE [sa: ComplexSequence] RETURNS [h: Handle] =
BEGIN
h ← SamplesToHandle[sa];
h.weight ← NEW[RealSequenceRec[h.n]];
FOR i:NAT IN [0..h.n) DO h.weight[i] ← 1.0 ENDLOOP;
h.t ← NEW[RealSequenceRec[h.n]];
END;
InitialKnots: PUBLIC PROCEDURE [h: Handle, nknots: NAT ← 2] =
BEGIN
h.knots ← NEW[RealSequenceRec[nknots]];
FOR i: NAT IN [0..nknots) DO
h.knots[i] ← i;
ENDLOOP;
END;
XYat: PUBLIC PROCEDURE [h: Handle, t: REAL] RETURNS [f: Complex.Vec] =
BEGIN
patchNumber: NAT ← 0;
interval: REAL;
IF h=NIL THEN RETURN[[-1,-1]];
WHILE patchNumber < h.knots.length-2 AND h.knots[patchNumber+1] < t DO
patchNumber ← patchNumber + 1;
ENDLOOP;
interval ← h.knots[patchNumber+1]-h.knots[patchNumber];
IF interval=0 THEN t←0
ELSE t←(t-h.knots[patchNumber])/interval;
{OPEN h.xPatches[patchNumber]; f.x ← ((c3*t+c2)*t+c1)*t+c0};
{OPEN h.yPatches[patchNumber]; f.y ← ((c3*t+c2)*t+c1)*t+c0};
END;
ClosestKnot: PUBLIC PROCEDURE [h: Handle, z: Complex.Vec]
RETURNS [NAT] =
BEGIN
dd: REAL ← 10E+20;
bestk:NAT ← 0;
FOR i:NAT IN (0..h.knots.length-1) DO
d: REAL ← Complex.SqrAbs[Complex.Sub[z,[h.xPatches[i].c0,h.yPatches[i].c0]]];
IF d<dd THEN {bestk←i; dd ← d}
ENDLOOP;
RETURN[bestk]
END;
SamplesToHandle: PROCEDURE [sa: ComplexSequence]
RETURNS [h: Handle] =
{h ← NEW[StateRec]; BEGIN OPEN h↑;
z ← sa;
n ← sa.length;
END};
BasisProc: TYPE = PROC [basisIndex: NAT, x: REAL] RETURNS [REAL];
FitOneDimensionalSpline: PROCEDURE
[X,Y,W: RealProc, l,n: NAT, -- points are numbered [l..n] (that's l, not 1)
Basis: BasisProc, nbasis: NAT] -- basis functions are numbered [0..nbasis)
RETURNS [LinearSystem.ColumnN] =
BEGIN OPEN LinearSystem;
b: ColumnN ← NEW[VecSeq[nbasis]];
c: ColumnN ← NIL;
A: MatrixN ← NEW[MatrixSeq[nbasis]];
FOR i: NAT IN [0..nbasis) DO
A[i] ← NEW[VecSeq[nbasis]];
FOR j: NAT IN [0..nbasis) DO A[i][j] ← 0; ENDLOOP;
b[i] ← 0;
ENDLOOP;
FOR k:NAT IN [l..n] DO
xk:REAL = X[k];
FOR i:NAT IN [0..nbasis) DO
gi:REAL ← Basis[i,xk];
IF gi#0 THEN
BEGIN
FOR j:NAT IN [0..nbasis) DO
gj: REAL ← Basis[j,xk];
A[i][j] ← A[i][j] + W[k]*gi*gj;
ENDLOOP;
b[i] ← b[i] + W[k]*Y[k]*gi;
END;
ENDLOOP;
ENDLOOP;
c ← SolveN[A,b,nbasis!
Real.RealException => CHECKED {CONTINUE}];
RETURN[c];
END;
FindPatches: PROCEDURE [h: Handle, c: LinearSystem.ColumnN]
RETURNS [C: PatchSequence]=
-- convert the coefficients for the basis functions to cubic patches
BEGIN OPEN h;
npatches: NAT ← knots.length-1;
C ← NEW[PatchSequenceRec[npatches]];
FOR n: NAT IN [0..npatches) DO
C[n] ← [0,0,0,0];
FOR i: NAT IN [0..c.ncols) DO
--most of these are all 0.
temp: Patch ← BasisCoeffs[h,i,n];
IF temp = [0,0,0,0] THEN LOOP; --for efficiency
C[n].c0 ← C[n].c0 + c[i]*temp.c0;
C[n].c1 ← C[n].c1 + c[i]*temp.c1;
C[n].c2 ← C[n].c2 + c[i]*temp.c2;
C[n].c3 ← C[n].c3 + c[i]*temp.c3;
ENDLOOP;
ENDLOOP;
END;
EvenL: Patch ← [c0: 0, c1: 0, c2: 3, c3: -2];
EvenR: Patch ← [c0: 1, c1: 0, c2: -3, c3: 2];
OddL: Patch ← [c0: 0, c1: 0, c2: -1, c3: 1];
OddR: Patch ← [c0: 0, c1: 1, c2: -2, c3: 1];
ScalePatch: PROC [p: Patch, r: REAL] RETURNS [Patch] =
BEGIN OPEN p;
RETURN[[c0:c0*r, c1:c1*r, c2:c2*r, c3:c3*r]]
END;
-- compute the value of the basis function i for the variable x
EvalBasis: PROC [h: Handle, i: NAT, x: REAL]
RETURNS [v: REAL] =
BEGIN OPEN h;
basis: Patch;
k0: NAT ← 0;
WHILE k0<knots.length-2 AND x>=knots[k0+1] DO k0 ← k0 + 1 ENDLOOP;
basis ← BasisCoeffs[h,i,k0]; --get the coefficients of the basis in this patch
{interval: REAL ← knots[k0+1]-knots[k0];
t: REAL ← IF interval=0 THEN 0 ELSE (x-knots[k0])/interval;
RETURN[t*(t*(t*basis.c3+basis.c2)+basis.c1)+basis.c0]};
END;
-- return the coefficients of the basis function i inside the specified patch
BasisCoeffs: PROCEDURE [h: Handle, i, patch: NAT]
RETURNS [p: Patch] =
BEGIN OPEN h;
leftHalf: NAT ← patch+1;
rightHalf: NAT ← patch;
IF closedCurve AND patch = knots.length-2 THEN leftHalf ← 0;
p ← SELECT i FROM
2*rightHalf => EvenR,
2*rightHalf+1 => ScalePatch[OddR,knots[patch+1]-knots[patch]],
2*leftHalf => EvenL,
2*leftHalf+1 => ScalePatch[OddL,knots[patch+1]-knots[patch]],
ENDCASE => [0,0,0,0];
END;
ImproveParametricSpline: PUBLIC PROCEDURE[h: Handle] =
BEGIN
AdjustTValues[h];
FitXAndY[h];
END;
InitialTValues: PUBLIC PROCEDURE [h: Handle] =
BEGIN OPEN h;
arcLen: REAL ← 0;
t[0] ← 0;
FOR i:NAT IN [1..n) DO
arcLen ← arcLen + Complex.Abs[Complex.Sub[z[i],z[i-1]]];
t[i] ← arcLen;
ENDLOOP;
FOR i:NAT IN [0..n) DO
t[i] ← knots[0] + t[i]*(knots[knots.length-1]-knots[0])/arcLen
ENDLOOP;
END;
AdjustTValues: PUBLIC PROCEDURE [h: Handle] =
BEGIN OPEN h↑;
k0: INTEGER ← 0;
delta,interval: REAL ← 0;
npatches: INTEGER ← knots.length-1;
IF knots=NIL OR t=NIL OR xPatches=NIL OR yPatches=NIL THEN RETURN;
FOR i: NAT IN [0..n) DO
WHILE t[i] > knots[k0+1] AND k0 < npatches-1 DO k0 ← k0+1 ENDLOOP;
WHILE k0>0 AND t[i] <= knots[k0] DO k0 ← k0-1 ENDLOOP;
interval ← knots[k0+1]-knots[k0];
IF interval # 0 THEN delta ← ImproveT[(t[i]-knots[k0])/interval,
xPatches[k0],yPatches[k0],z[i].x,z[i].y];
t[i] ← t[i]+delta*interval;
IF closedCurve THEN
BEGIN
WHILE t[i] < knots[0] DO
t[i] ← t[i] + (knots[npatches]-knots[0]) ENDLOOP;
WHILE t[i] >= knots[npatches] DO
t[i] ← t[i] - (knots[npatches]-knots[0]) ENDLOOP;
END;
ENDLOOP;
-- Sort[t];
IF NOT closedCurve THEN
BEGIN
knots[0] ← t[0];
knots[npatches] ← t[n-1]
END;
-- Sort[knots];
END;
FitXAndY: PUBLIC PROCEDURE [h: Handle] =
BEGIN OPEN h;
X:RealProc = {RETURN[z[i].x]};
Y:RealProc = {RETURN[z[i].y]};
W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
T:RealProc = {RETURN[t[i]]};
nbasis: NAT ← 2*knots.length;
Phi: BasisProc = {RETURN[EvalBasis[h, basisIndex, x]]};
IF closedCurve THEN nbasis ← nbasis - 2;
xPatches ← FindPatches
[h, FitOneDimensionalSpline[T,X,W,0,n-1,Phi,nbasis]];
yPatches ← FindPatches
[h, FitOneDimensionalSpline[T,Y,W,0,n-1,Phi,nbasis]];
END;
ImproveT: PROCEDURE [ti: REAL, X, Y: Patch, XI,YI: REAL] RETURNS [delta: REAL] =
BEGIN
FX: PROC [t:REAL] RETURNS [REAL] = INLINE
{RETURN[X.c0 + t*(X.c1 + t*(X.c2 + t*(X.c3)))]};
DFX: PROC [t:REAL] RETURNS [REAL] = INLINE
{RETURN[X.c1 + t*(2*X.c2 + t*(3*X.c3))]};
DDFX: PROC [t:REAL] RETURNS [REAL] = INLINE
{RETURN[2*X.c2+ t*(2*3*X.c3)]};
FY: PROC [t:REAL] RETURNS [REAL] = INLINE
{RETURN[Y.c0 + t*(Y.c1 + t*(Y.c2 + t*(Y.c3)))]};
DFY: PROC [t:REAL] RETURNS [REAL] = INLINE
{RETURN[Y.c1 + t*(2*Y.c2 + t*(3*Y.c3))]};
DDFY: PROC [t:REAL] RETURNS [REAL] = INLINE
{RETURN[2*Y.c2 + t*(2*3*Y.c3)]};
derivSqrDist:REAL = (FX[ti]-XI)*DFX[ti] + (FY[ti]-YI)*DFY[ti];
derivDerivSqrDist:REAL =
(DFX[ti])*(DFX[ti]) + (FX[ti]-XI)*DDFX[ti] +
(DFY[ti])*(DFY[ti]) + (FY[ti]-YI)*DDFY[ti];
IF derivDerivSqrDist # 0 THEN delta ← -derivSqrDist / derivDerivSqrDist
ELSE delta ← 0;
END;
Sort: PUBLIC PROCEDURE [v: RealSequence] = {
--sorts v sequence in place. Would be awful except that sequence is mostly ordered already
l: NAT ← v.length-1;
nswitches: NAT ← 1;
UNTIL nswitches=0 DO
nswitches ← 0;
FOR i: NAT IN [0..l) DO
IF v[i+1] < v[i] THEN {
t: REAL ← v[i+1];
v[i+1] ← v[i];
v[i] ← t;
nswitches ← nswitches+1;
}
ENDLOOP;
ENDLOOP;
};
ReParameterize: PUBLIC PROC [p: Patch, t0,t1: REAL, nt0,nt1: REAL] RETURNS[np: Patch] = { OPEN LinearSystem;
A: Matrix4;
B: Column4;
C: Column4;
F: PROC [t: REAL] RETURNS[REAL] = {RETURN[p.c0 + t*(p.c1 + t*(p.c2 + t*(p.c3)))]};
DF: PROC [t: REAL] RETURNS[REAL] = {RETURN[p.c1 + t*(2*p.c2 + t*(3*p.c3))]};
r: REAL ← (t1-t0)/(nt1-nt0);
A[1] ← [nt1*nt1*nt1, nt1*nt1, nt1, 1];
A[2] ← [nt0*nt0*nt0, nt0*nt0, nt0, 1];
A[3] ← [3*nt1*nt1, 2*nt1, 1, 0];
A[4] ← [3*nt0*nt0, 2*nt0, 1, 0];
B[1] ← F[t1];
B[2] ← F[t0];
B[3] ← r*DF[t1];
B[4] ← r*DF[t0];
C ← Solve4[A,B];
np ← [c3: C[1],c2: C[2],c1: C[3],c0: C[4]];
};
FitOneDimensionalCubic: PUBLIC PROCEDURE [X,Y,W: RealProc, l,n:NAT]
RETURNS [Patch] =
BEGIN -- finds cubic f(X) to minimize SUM (f(X[i])-Y[i])↑2 OVER l <= i <= n
singular:BOOLEAN ← FALSE;
b,c:LinearSystem.Column4 ← [0,0,0,0];
A:LinearSystem.Matrix4 ← [[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0]];
FOR i:NAT IN [l..n] DO
xi:REAL = X[i];
r:REAL ← 1;
FOR k:NAT IN [1..4] DO
s:REAL ← 1;
FOR j:NAT IN [1..4] DO
A[k][j] ← A[k][j] + W[i]*r*s;
s ← s*xi
ENDLOOP;
b[k] ← b[k] + W[i]*Y[i]*r;
r ← r*xi
ENDLOOP;
ENDLOOP;
c ← LinearSystem.Solve4[A,b!
Real.RealException => CHECKED {singular←TRUE;CONTINUE}];
IF singular THEN RETURN [[0,1,0,0]]
ELSE RETURN [[c3:c[4],c2:c[3],c1:c[2],c0:c[1]]]
END;
InitialParametricCubic: PUBLIC PROCEDURE[sa: ComplexSequence]
RETURNS [h: Handle] =
{h ← SamplesToHandle[sa]; BEGIN OPEN h↑;
X:RealProc = {RETURN[z[i].x]};
Y:RealProc = {RETURN[z[i].y]};
W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
T:RealProc = {RETURN[t[i]]};
arcLen:REAL ← 0;
t ← NEW[RealSequenceRec[n]];
t[0] ← 0;
FOR i:NAT IN [1..n) DO
arcLen ← arcLen + Complex.Abs[Complex.Sub[z[i],z[i-1]]];
t[i] ← arcLen;
ENDLOOP;
FOR i:NAT IN [0..n) DO
t[i] ← t[i] / arcLen
ENDLOOP;
knots ← NEW[RealSequenceRec[2]];
knots[0] ← 0;
knots[1] ← 1;
xPatches ← NEW[PatchSequenceRec[1]];
yPatches ← NEW[PatchSequenceRec[1]];
xPatches[0] ← FitOneDimensionalCubic[T,X,W,0,n-1];
yPatches[0] ← FitOneDimensionalCubic[T,Y,W,0,n-1];
END};
ImproveParametricCubic: PUBLIC PROCEDURE [h: Handle, first: NAT ← 0, last: NAT ← LAST[NAT]] =
BEGIN OPEN h↑;
X:RealProc = {RETURN[z[i].x]};
Y:RealProc = {RETURN[z[i].y]};
W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
T:RealProc = {RETURN[t[i]]};
range: REAL;
IF last>=n THEN last ← n-1;
range ← t[last]-t[first];
FOR i:NAT IN [first..last] DO
t[i] ← t[i] + ImproveT[t[i],xPatches[0],yPatches[0],z[i].x,z[i].y];
ENDLOOP;
{t0,tn1:REAL;
t0 ← t[first];
tn1 ← t[last];
IF tn1 # t0 THEN FOR i:NAT IN [first..last] DO
t[i] ← (t[i]-t0) * range / (tn1 - t0)
ENDLOOP};
xPatches[0] ← FitOneDimensionalCubic[T,X,W,first,last];
yPatches[0] ← FitOneDimensionalCubic[T,Y,W,first,last];
END;
FitParametricCubic: PUBLIC PROCEDURE [h: Handle, first, last: NAT, epsilon: REAL] =
BEGIN OPEN h;
X:RealProc = {RETURN[z[i].x]};
Y:RealProc = {RETURN[z[i].y]};
W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
T:RealProc = {RETURN[t[i]]};
arcLen:REAL ← 0;
error: REAL ← epsilon+1;
t ← NEW[RealSequenceRec[n]];
t[0] ← 0;
FOR i:NAT IN [1..n) DO
arcLen ← arcLen + Complex.Abs[Complex.Sub[z[i],z[i-1]]];
t[i] ← arcLen;
ENDLOOP;
{t0,tn1:REAL;
t0 ← t[first];
tn1 ← t[last];
IF tn1 # t0 THEN FOR i:NAT IN [first..last] DO
t[i] ← (t[i]-t0) / (tn1 - t0)
ENDLOOP};
knots ← NEW[RealSequenceRec[2]];
knots[0] ← 0;
knots[1] ← 1;
xPatches ← NEW[PatchSequenceRec[1]];
yPatches ← NEW[PatchSequenceRec[1]];
xPatches[0] ← FitOneDimensionalCubic[T,X,W,first,last];
yPatches[0] ← FitOneDimensionalCubic[T,Y,W,first,last];
THROUGH [0..400) WHILE error>=epsilon DO
error ← 0;
FOR i:NAT IN [first..last] DO
delta: REAL ← ImproveT[t[i],xPatches[0],yPatches[0],z[i].x,z[i].y];
t[i] ← t[i] + delta;
error ← error + ABS[delta];
ENDLOOP;
{t0,tn1:REAL;
t0 ← t[first];
tn1 ← t[last];
IF tn1 # t0 THEN FOR i:NAT IN [first..last] DO
t[i] ← (t[i]-t0) / (tn1 - t0)
ENDLOOP};
xPatches[0] ← FitOneDimensionalCubic[T,X,W,first,last];
yPatches[0] ← FitOneDimensionalCubic[T,Y,W,first,last];
ENDLOOP;
END;
END.