LSFitImpl.mesa
Copyright © 1985 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𡤀
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.