DynFitImpl.mesa
Copyright Ó 1985, 1992 by Xerox Corporation. All rights reserved.
Michael Plass 30-Sep-81
Last edited by Michael Plass August 23, 1982 9: 18 am
Maureen Stone September 14, 1984 5: 15: 37 pm PDT
Doug Wyatt, September 5, 1985 2: 27: 51 pm PDT
Michael Plass, December 16, 1985 4:41:09 pm PST
Maureen Stone, October 2, 1987 4:36:18 pm PDT
DIRECTORY
Complex USING [Abs, Add, Mul, Sqr, SqrAbs, SqRt, Sub, VEC],
CedarProcess USING [CheckAbort],
DynFit USING [Line, PointNumber, Segments, SegmentSequenceRec],
Seq USING [ComplexSequence, ComplexSequenceRec, NatSequence, NatSequenceRec, RealSequence, RealSequenceRec],
Vector USING [Div, Dot, Mul];
DynFitImpl: CEDAR PROGRAM
IMPORTS Vector, Complex, CedarProcess
EXPORTS DynFit =
BEGIN OPEN Seq;
PointNumber: TYPE = DynFit.PointNumber;
These global variables hold the samples
NearestZ:
PUBLIC
PROCEDURE [z: ComplexSequence, v: Complex.
VEC]
RETURNS [PointNumber] =
BEGIN
closest: PointNumber ¬ 0;
best: REAL ¬ 10e+20;
n: NAT ¬ z.length;
IF z#
NIL
THEN
FOR i:
NAT
IN [0..n)
DO
dd: REAL = Complex.SqrAbs[Complex.Sub[v, z[i]]];
IF dd < best THEN {best ¬ dd; closest ¬ i}
ENDLOOP;
RETURN[closest];
END;
LeastSquares:
PROCEDURE [numberOfPoints:
REAL,
sum, sumSqr: Complex.VEC,
sumSqrAbs: REAL] RETURNS [REAL] =
BEGIN -- finds the sum of the squares of the distances of a set of points to a line, where the line is chosen such that this sum is minimized. The points are specified as complex numbers, and this routine requires their (complex) sum and the sum of their (complex) squares, as well as the sum of the squares of their absolute values. For the best numerical stability, the origin should be chosen as close to the center of mass of the points as is practical.
OPEN Complex;
RETURN[(sumSqrAbs - SqrAbs[sum]/numberOfPoints
- Abs[Sub[sumSqr, Vector.Div[Sqr[sum], numberOfPoints]]])/2]
END;
BestLine:
PUBLIC
PROCEDURE [z: ComplexSequence, firstPoint, lastPoint: PointNumber]
RETURNS [r: DynFit.Line] =
BEGIN -- finds the line that minimizes the sum of the squares of the distances to the points. The equation of the resulting line is given by centerOfMass + t*direction, where t is a real-valued parameter.
OPEN Complex;
n: NAT = z.length;
wrap: PROC[i: NAT] RETURNS[NAT] = INLINE {RETURN[IF i>=n THEN i-n ELSE i]};
f: NAT = IF firstPoint<=lastPoint THEN firstPoint ELSE lastPoint;
l: NAT = lastPoint + firstPoint - f;
m: NAT ¬ l - f + 1;
a: Complex.VEC ¬ [0, 0];
v: Complex.VEC ¬ [0, 0];
IF m > n THEN m ¬ n;
FOR i:
NAT
IN [f..l]
DO
a ¬ Add[a, z[wrap[i]]];
ENDLOOP;
a ¬ Vector.Div[a, m];
FOR i:
NAT
IN [f..l]
DO
v ¬ Add[v, Sqr[Sub[z[wrap[i]], a]]];
ENDLOOP;
v ¬ Vector.Div[v, m];
r.centerOfMass ¬ a;
r.direction ¬ SqRt[v];
END;
tryAll: BOOLEAN ¬ FALSE;
FitSegments:
PUBLIC
PROCEDURE [z: ComplexSequence, closed:
BOOLEAN, tolerance:
REAL]
RETURNS [segments: DynFit.Segments, totalBadness: REAL] =
BEGIN
n: NAT ¬ IF closed THEN z.length+1 ELSE z.length;
wrap: PROC[i: NAT] RETURNS[NAT] = INLINE {RETURN[IF i>=z.length THEN i-z.length ELSE i]};
bestPrev: NatSequence = NEW[NatSequenceRec[n]];
segCount: NatSequence = NEW[NatSequenceRec[n]];
totSqrD: RealSequence = NEW[RealSequenceRec[n]];
IF n<2 THEN RETURN [NIL, 0];
bestPrev[0] ¬ 0;
segCount[0] ¬ 0;
totSqrD[0] ¬ 0.0;
FOR m:
NAT
IN [1..n)
DO
sum, sumSqr: Complex.VEC ¬ [0, 0];
sumSqrAbs: REAL ¬ 0;
b: REAL ¬ 0;
lowbad: REAL ¬ 1.0E+20;
bestl: NAT;
s: REAL ¬ 0;
sOpt: REAL;
sBound: REAL ¬ 0.001;
FOR l:
NAT
DECREASING
IN [0..m)
WHILE tryAll
OR s<=sBound
DO
deltaZ: Complex.VEC = Complex.Sub[z[l], z[wrap[m]]];
sum ¬ Complex.Add[sum, deltaZ];
sumSqr ¬ Complex.Add[sumSqr, Complex.Sqr[deltaZ]];
sumSqrAbs ¬ sumSqrAbs + Complex.SqrAbs[deltaZ];
s ¬ LeastSquares[m-l+1, sum, sumSqr, sumSqrAbs];
b ¬ s + totSqrD[l] + tolerance * (segCount[l] + 1);
sBound ¬ sBound + tolerance;
IF b<lowbad THEN {lowbad¬b; bestl¬l; sOpt¬s}
ENDLOOP;
totSqrD[m] ¬ sOpt + totSqrD[bestl];
segCount[m] ¬ segCount[bestl] + 1;
bestPrev[m] ¬ bestl;
CedarProcess.CheckAbort[];
ENDLOOP;
totalBadness ¬ totSqrD[n-1] + tolerance * (segCount[n-1] + 1);
Now record the answer
BEGIN
p: NAT;
m: NAT ¬ segCount[n-1] + 1;
segments ¬ NEW[DynFit.SegmentSequenceRec[m]];
p ¬ n-1;
FOR i:
NAT
DECREASING
IN [0..m)
DO
segments[i] ¬ p;
p ¬ bestPrev[p]
ENDLOOP
END
END;
SampleSegments:
PUBLIC
PROCEDURE [segments: DynFit.Segments, z: ComplexSequence]
RETURNS [newSamples: ComplexSequence, len: NAT] =
BEGIN -- Make new samples from the segmentation info.
epsilon: REAL ¬ 1.0;
wrap: PROC[i: NAT] RETURNS[NAT] = INLINE {RETURN[IF i>=z.length THEN i-z.length ELSE i]};
Sample:
PROCEDURE [v: Complex.
VEC] =
BEGIN
newSamples[len] ¬ v;
len ¬ len + 1;
END;
newSamples ¬ NEW [ComplexSequenceRec[segments.length*2]];
len ¬ 0;
FOR m:
NAT
DECREASING
IN (0..segments.length)
DO
OPEN Complex; -- uses VEC, Add, Sub, Mul, Abs
first: NAT ¬ segments[m-1];
last: NAT ¬ segments[m];
line: DynFit.Line ¬ BestLine[z, first, last];
towardsLast: VEC ¬ Sub[z[wrap[last]], line.centerOfMass];
t:
VEC =
-- tangent vector directed roughly toward last point
Vector.Mul[line.direction, Vector.Dot[line.direction, towardsLast]];
s: REAL = Abs[t];
unit: VEC = IF s>0 THEN Vector.Div[t, s] ELSE t;
normal: VEC = Mul[unit, [0, 1]];
max, min: REAL ¬ 0; -- extreme components along tangent
oldNormalComp, oldTangentComp: REAL ¬ 0;
FOR i:
NAT
IN [segments[m-1]..segments[m]]
DO
zi: VEC = Sub[z[wrap[i]], line.centerOfMass];
tangentComp: REAL = Vector.Dot[zi, unit];
normalComp: REAL = Vector.Dot[zi, normal];
candidate: REAL ¬ 0;
IF normalComp < epsilon THEN candidate ¬ tangentComp
ELSE
IF oldNormalComp*normalComp < 0
THEN
-- find crossing
candidate ¬ (normalComp*oldTangentComp
- oldNormalComp*tangentComp)
/ (normalComp - oldNormalComp);
IF candidate < min THEN min ¬ candidate;
IF candidate > max THEN max ¬ candidate;
oldNormalComp ¬ normalComp;
oldTangentComp ¬ tangentComp;
ENDLOOP;
Sample[Add[line.centerOfMass, Vector.Mul[unit, max]]];
Sample[Add[line.centerOfMass, Vector.Mul[unit, min]]];
ENDLOOP;
END;
END.
Michael Plass August 23, 1982 9: 18 am. Added check for jam break. Removed CountSamples and SamplesToZ.