DynFitImpl.mesa
Copyright © 1985 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
DIRECTORY
Complex USING [Abs, Add, Mul, Sqr, SqrAbs, SqRt, Sub, VEC],
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
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: BOOLEANFALSE;
FitSegments: PUBLIC PROCEDURE [z: ComplexSequence, closed: BOOLEAN, tolerance: REAL]
RETURNS [segments: DynFit.Segments, totalBadness: REAL] =
BEGIN
n: NATIF 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𡤋 bestl←l; sOpt←s}
ENDLOOP;
totSqrD[m] ← sOpt + totSqrD[bestl];
segCount[m] ← segCount[bestl] + 1;
bestPrev[m] ← bestl;
IF JaMFnsDefs.GetJaMBreak[] THEN {n←m+1; EXIT};
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.