LSFitImpl.mesa
Copyright Ó 1985, 1992 by Xerox Corporation. All rights reserved.
Michael Plass and Maureen Stone Oct-81
Last edited by Michael Plass 10-Mar-82 15:23:07
Doug Wyatt, September 5, 1985 1:26:38 pm PDT
Bier, July 21, 1987 1:44:00 pm PDT
DIRECTORY
Complex USING [Abs, SqrAbs, Sub, VEC],
LinearSystem USING [Column4, ColumnN, Matrix4, MatrixN, MatrixSeq, Solve4, SolveN, VecSeq],
LSFit USING [ComplexSequence, ComplexSequenceRec, Handle, Patch, PatchSequence, PatchSequenceRec, RealSequence, RealSequenceRec, StateRec],
Real USING [RealException];
LSFitImpl: CEDAR 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;
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[0] ¬ [nt1*nt1*nt1, nt1*nt1, nt1, 1];
A[1] ¬ [nt0*nt0*nt0, nt0*nt0, nt0, 1];
A[2] ¬ [3*nt1*nt1, 2*nt1, 1, 0];
A[3] ¬ [3*nt0*nt0, 2*nt0, 1, 0];
B[0] ¬ F[t1];
B[1] ¬ F[t0];
B[2] ¬ r*DF[t1];
B[3] ¬ r*DF[t0];
C ¬ Solve4[A,B];
np ¬ [c3: C[0], c2: C[1], c1: C[2], c0: C[3]];
};
FitOneDimensionalCubic:
PUBLIC
PROC [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[3], c2:c[2], c1:c[1], c0:c[0]]]
END;
InitialParametricCubic:
PUBLIC
PROC [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.