PiecewiseFitImpl.mesa
last edited by Maureen Stone December 13, 1984 12:39:40 pm PST
last edited by Michael Plass August 30, 1982 1:42 pm
DIRECTORY
Complex,
Cubic,
LSPiece,
Rope USING [ROPE],
Seq,
Vector,
PiecewiseFit,
Real,
RealFns;
PiecewiseFitImpl: CEDAR PROGRAM
IMPORTS LSPiece, Vector, Real
EXPORTS PiecewiseFit =
BEGIN OPEN PiecewiseFit;
Metrics: TYPE = LSPiece.Metrics;
Error: PUBLIC SIGNAL[Rope.ROPE] = CODE;
Default: PUBLIC PROC [data: Data] = {
IF data=NIL OR data.samples=NIL OR data.samples.length=0 THEN
SIGNAL Error["No samples"];
IF data.joints=NIL OR data.joints.length=0 THEN {
IF data.closed THEN {
data.joints ← NEW[Seq.JointSequenceRec[1]];
data.joints[0] ← [0, FALSE];
data.tangents ← NEW[Seq.ComplexSequenceRec[2]];
data.tangents[0] ← [0,0];
data.tangents[1] ← [0,0];
}
ELSE {
data.joints ← NEW[Seq.JointSequenceRec[2]];
data.joints[0] ← [0, FALSE]; data.joints[1] ← [data.samples.length-1, FALSE];
data.tangents ← NEW[Seq.ComplexSequenceRec[4]];
data.tangents[0] ← [0,0]; data.tangents[1] ← [0,0];
data.tangents[2] ← [0,0]; data.tangents[3] ← [0,0];
};
};
};
The following routines deal in node indexes and corner indexes.
From and thru for fitting are sample indexes, ie: joints[startJoint].index, joints[endJoint].index
Init: PROC [data: Data] RETURNS [nPieces: INT, firstJ: INT] = {
IF data=NIL OR data.samples=NIL OR data.samples.length=0 THEN SIGNAL Error["No samples"];
IF data.joints=NIL OR data.joints.length=0 THEN SIGNAL Error["No joints"];
IF data.tangents=NIL OR data.tangents.length=0 THEN SIGNAL Error["No tangents"];
IF NOT data.closed THEN {
IF data.joints.length <4 THEN SIGNAL Error["Not enough joints on open curve"];
IF data.joints[0].index#0 OR data.joints[data.joints.length-1].index#data.samples.length-1
THEN SIGNAL Error["No joints on endpoints"];
};
IF 2*data.joints.length#data.tangents.length THEN
SIGNAL Error["tangents.length#2*joints.length"];
Now we can assume well formed data.
firstJ 0;
IF data.closed THEN {
FOR i: INT IN [0..data.joints.length) DO
IF data.joints[i].forced THEN {firstJ ← i; EXIT};
ENDLOOP;
RETURN [data.joints.length,firstJ];
}
ELSE RETURN [data.joints.length-1, firstJ];
};
joint indexes
Next: PROC [data: Data, joint: INT] RETURNS [INT] = {
joint ← joint+1;
IF joint >= data.joints.length THEN
IF data.closed THEN RETURN[0]
ELSE {SIGNAL Error["error: ran off the end of samples"]; RETURN[joint-1]}
ELSE RETURN[joint];
};
Prev: PROC [data: Data, joint: INT] RETURNS [INT] = {
joint ← joint-1;
IF joint < 0 THEN
IF data.closed THEN RETURN[data.joints.length-1]
ELSE {SIGNAL Error["error: ran off the end of samples"]; RETURN[joint+1]}
ELSE RETURN[joint];
};
SelectMidJoint: PROC [data: Data, start: INT] RETURNS [INT] = {
find a good place for a joint about half-way around
njoints: NAT ← data.joints.length;
mid: NAT ← start;
IF njoints<=2 THEN SIGNAL Error["Too few joints"];
FOR j: NAT IN [0..njoints/2) DO mid ← Next[data, j]; ENDLOOP;
RETURN[mid];
};
FitSpline: PUBLIC PROC [data: Data, metrics: Metrics, outputCubic: CubicProc] = {
nPieces, start, end: INT;
t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[data.samples.length]];
[nPieces, start] ← Init[data];
end ← Next[data,start];
THROUGH [0..nPieces) DO
b: Cubic.Bezier;
err,maxd: REAL;
iterations: NAT;
[b,err,iterations] ←
FitPieceInterp[z: data.samples, t: t,
from: data.joints[start].index, thru: data.joints[end].index,
metrics: metrics,
initFree: FALSE, finalFree: FALSE,
initTangent: data.tangents[2*start+1], finalTangent: data.tangents[2*end]];
IF outputCubic[bezier: b, sumErr: err, maxDev: maxd, iterations: iterations]
THEN GOTO aborted;
start ← end;
end ← Next[data, end];
ENDLOOP;
EXITS aborted => NULL;
};
GrowSpline: PUBLIC PROC[data: Data, metrics: Metrics, outputCubic: CubicProc, hilight: CubicProc ← NIL] = {
pstart,pend: NAT ← 0;
t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[data.samples.length]];
b: Cubic.Bezier;
maxMetrics: LSPiece.Metrics ← NEW[LSPiece.MetricsRec ← [ --for final fit
maxItr: metrics.maxItr, maxDev: 0, sumErr: 0, deltaT: 0]];
iterations: NAT;
err, maxdev, prevErr: REAL ← 0;
t1,t2: Complex.Vec;
nPieces, start, last: INT;
abort: SIGNAL = CODE;
doFinal: PROC [start, end: NAT] = {
[b,err,iterations,maxdev] ← FitPieceInterp[z: data.samples, t: t,
from: data.joints[start].index, thru: data.joints[end].index,
metrics: maxMetrics,
initFree: FALSE, finalFree: FALSE,
initTangent: t1, finalTangent: data.tangents[2*pend]];
IF outputCubic[bezier: b, sumErr: err, maxDev: maxdev, iterations: iterations]
THEN SIGNAL abort;
};
[nPieces, start] ← Init[data];
last ← IF data.closed THEN start ELSE data.joints.length-1;
pstart ← start; --start guaranteed to be a corner if there is one
DO
t1 ← data.tangents[2*pstart+1];
pend ← Next[data,pstart];
DO
t2 ← data.tangents[2*pend];
[b,err,iterations,maxdev] ← FitPieceInterp[z: data.samples, t: t,
from: data.joints[pstart].index, thru: data.joints[pend].index,
metrics: metrics,
initFree: FALSE, finalFree: FALSE,
initTangent: t1, finalTangent: t2];
IF hilight#NIL
THEN IF hilight[bezier: b, sumErr: err, maxDev: maxdev, iterations: iterations]
THEN GOTO aborted;
IF maxdev>metrics.maxDev THEN { --back up if possible
temp: INT ← Prev[data,pend];
IF temp#pstart THEN pend ← temp;
EXIT;
};
IF data.joints[pend].forced OR pend=last THEN EXIT;
pend ← Next[data,pend];
ENDLOOP;
It may seem possible to fit a piece with a single cubic. This will not usually work. Special case it here. I expect this to happen when a small closed piece is being fit
IF pend=pstart THEN {
mid: NAT ← SelectMidJoint[data, pstart];
doFinal[pstart, mid ! abort => GOTO aborted];
pstart ← mid;
};
do one more fit with the maximum number of iterations to guarentee the best cubic
doFinal[pstart, pend ! abort => GOTO aborted];
IF pend=last THEN EXIT;
pstart ← pend;
ENDLOOP;
EXITS aborted => NULL;
};
CostRec: TYPE = RECORD [
subtotal: REAL,
bestPrev: INT ← -1, --index into cost record
joint: INT ← -1, --index into joint record
tIn,tOut: Complex.Vec ← [0,0]
];
CostSeq: TYPE = RECORD[element: SEQUENCE length: NAT OF CostRec];
DynSpline: PUBLIC PROC[data: Data, metrics: Metrics, penalty: REAL, trim: BOOLEAN ← TRUE, outputCubic: CubicProc, hilight: CubicProc ← NIL] =
BEGIN
t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[data.samples.length]];
bezier: Cubic.Bezier;
cost: REF CostSeq;
start, end: INT ← 0;
abort: ERROR = CODE;
OutputPatchesUpTo: PROC [m: NAT] = { --output the best patches
sumErr, maxDev: REAL;
iterations: INT;
IF m>0 THEN {
OutputPatchesUpTo[cost[m].bestPrev];
[b: bezier, err: sumErr, iterations: iterations,maxDev: maxDev] ← FitPieceInterp[
z: data.samples, t: t,
from: data.joints[cost[cost[m].bestPrev].joint].index,
thru: data.joints[cost[m].joint].index,
metrics: metrics,
initFree: FALSE, finalFree: FALSE,
initTangent: cost[cost[m].bestPrev].tOut,
finalTangent: cost[m].tIn
];
IF outputCubic[bezier: bezier, sumErr: sumErr, maxDev: maxDev, iterations: iterations] THEN ERROR abort; --blasts out of recursion on user abort
};
};
[, start] ← Init[data];
end ← start;
cost ← NEW[CostSeq[(IF data.closed THEN data.joints.length+1 ELSE data.joints.length)]];
cost[0] holds tangents, joint index and subtotal. bestPrev is meaningless
cost[0] ← [subtotal: 0.0, bestPrev: -1, joint: start, tIn: data.tangents[2*start], tOut: data.tangents[2*start+1]];
Compute the cost for each joint. i is the index into cost and corresponds to the joint index
end is the joint index for the far end, prev moves back along the curve
FOR i: INT IN [1..cost.length) DO
maxDev: REAL;
iterations: INT;
badness: REAL;
lowbad:REALLAST[INT]/2;
bestPrev: INT ← 0;
sumErr:REAL ← 0;
end ← Next[data, end];
{ --now work out tangents (t1, t2) for this pair of ends (prev, end)
t1,t2,bestTOut: Complex.Vec;
prev: INT ← Prev[data, end];
t2 ← data.tangents[2*end];
FOR prevM: INT DECREASING IN [0..i-1] DO
t1 ← data.tangents[2*prev+1];
[b: bezier, err: sumErr, iterations: iterations,maxDev: maxDev] ←
FitPieceInterp[z: data.samples, t: t,
from: data.joints[prev].index, thru: data.joints[end].index,
metrics: metrics,
initFree: FALSE, finalFree: FALSE,
initTangent: t1,
finalTangent: t2
];
IF hilight#NIL THEN
IF hilight[bezier: bezier, sumErr: sumErr, maxDev: maxDev, iterations: iterations] THEN GOTO aborted;
badness ← maxDev+cost[prevM].subtotal + MAX[(maxDev-metrics.maxDev)*penalty,0];
penalize for over maxDev. If you're sure that you can always fit within maxDev make the penalty very large.
IF badness < lowbad THEN {
lowbad ← badness;
bestPrev ← prevM;
bestTOut ← t1;
};
IF data.joints[prev].forced OR prev=start THEN EXIT; --don't look for solutions beyond previous corner or start of samples
IF trim AND maxDev > trimfactor*metrics.maxDev THEN EXIT; --heuristic for efficiency
prev ← Prev[data, prev];
IF prev=end THEN EXIT; --Can't fit whole closed curve at once. should fix LSPiece
ENDLOOP;
cost[bestPrev].tOut ← bestTOut;
cost[i] ← [subtotal: (IF data.joints[end].forced THEN 0 ELSE lowbad), bestPrev: bestPrev, joint: end, tIn: t2];
};
ENDLOOP;
OutputPatchesUpTo[cost.length-1 ! abort => GOTO aborted];
EXITS aborted => NULL; --abort exit
END;  
trimfactor: REAL ← 1.5;
FitPiece: PUBLIC PROC [data: Data, from, thru: NAT, initFree, finalFree: BOOLEANTRUE, metrics: Metrics, outputCubic: CubicProc] = {
bezier: Cubic.Bezier;
sumErr, maxDev: REAL;
iterations: INT;
t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[data.samples.length]];
initTangent, finalTangent: Complex.Vec ← [0,0];
IF data.joints#NIL AND data.tangents#NIL THEN {
fromJoint, thruJoint: INT ← -1;
FOR i: NAT IN [0..data.joints.length) DO
IF data.joints[i].index=from THEN fromJoint←i;
IF data.joints[i].index=thru THEN thruJoint←i;
ENDLOOP;
IF fromJoint # -1 THEN initTangent ← data.tangents[2*fromJoint+1];
IF thruJoint # -1 THEN finalTangent ← data.tangents[2*thruJoint];
};
[b: bezier, err: sumErr, iterations: iterations, maxDev: maxDev] ← FitPieceInterp[z: data.samples, t: t,
from: from, thru: thru,
metrics: metrics,
initFree: initFree, finalFree: finalFree,
initTangent: initTangent, finalTangent: finalTangent,
useOldTValues: FALSE];
[] ← outputCubic[bezier, sumErr, maxDev, iterations];
};
Like LSPiece.FitPiece but will interpolate the points if necessary (need at least 6 for a fit)
FitPieceInterp: PROCEDURE
[z: Seq.ComplexSequence, t: Seq.RealSequence ← NIL,
from, thru: NAT, -- fit points in range [from..thru] modulo z.length
metrics: Metrics,
initFree, finalFree: BOOLEANFALSE,
initTangent, finalTangent: Complex.Vec ← [0,0],
useOldTValues: BOOLEANFALSE
]
RETURNS [b: Cubic.Bezier, err: REAL, iterations: NAT, maxDev: REAL] = {
this makes sure there are enough points to give a good fit, then calls LSPiece.FitPiece
npoints: NATIF thru>from THEN thru-from+1 ELSE thru-from+z.length+1;
minPoints: NAT ← 6;
IF npoints>=minPoints THEN {
[b,err,iterations, maxDev] ← LSPiece.FitPiece[z: z, t: t, from: from, thru: thru,
metrics: metrics,
initFree: initFree, finalFree: finalFree,
initTangent: initTangent,
finalTangent: finalTangent, useOldTValues: useOldTValues];
}
ELSE { --else interpolate and then call
newZ: Seq.ComplexSequence ← NEW[Seq.ComplexSequenceRec[minPoints]];
newT: Seq.RealSequence ← NEW[Seq.RealSequenceRec[minPoints]];
ninterp: INT ← minPoints-npoints;
zi: NAT ← 0;
modZ: PROC[index: NAT] RETURNS [NAT] = {RETURN[index MOD z.length]};
interpolate: PROC[p0,p1: Vector.Vec, npts: NAT] = {
dv: Vector.Vec ← Vector.Mul[Vector.Sub[p1,p0],1.0/(npts+1)];
FOR j: NAT IN [0..npts) DO
newZ[zi] ← Vector.Add[newZ[zi-1],dv];
zi ← zi+1;
ENDLOOP;
};
newZ[0] ← z[from];
IF npoints=2 THEN {zi ← 1; interpolate[p0: z[from], p1: z[thru], npts: ninterp]}
ELSE {
d: Seq.RealSequence ← NEW[Seq.RealSequenceRec[npoints-1]];
minD: REAL ← 100000000;
maxD: REAL ← 0;
totalD: REAL ← 0;
np: NAT ← 0;
i,n: NAT ← 0;
adjust: BOOLEANFALSE;
FOR i ← from, modZ[i+1] UNTIL i=thru DO
d[n] ← Vector.Mag[Vector.Sub[z[i],z[modZ[i+1]]]];
totalD ← totalD+d[n];
IF d[n]<minD THEN minD ← d[n];
IF d[n]>maxD THEN maxD ← d[n];
n ← n+1;
ENDLOOP;
FOR i IN [0..d.length) DO np ← np+Real.RoundI[(d[i]/totalD)*ninterp]; ENDLOOP;
IF np#ninterp THEN adjust ← TRUE;
FOR i IN [0..d.length) DO
p: INTEGER ← Real.RoundI[(d[i]/totalD)*ninterp];
newZ[zi] ← z[modZ[from+i]]; zi ← zi+1;
IF adjust THEN{
IF np>ninterp AND d[i]=minD THEN {p←p-1; np ← np-1};
IF np<ninterp AND d[i]=maxD THEN {p←p+1; np ← np+1};
IF np=ninterp THEN adjust ← FALSE}; --should only be off by one
IF p>0 THEN interpolate[p0: z[modZ[from+i]], p1: z[modZ[from+i+1]], npts: p];
ENDLOOP;
};
newZ[newZ.length-1] ← z[thru];
[b,err,iterations, maxDev] ← LSPiece.FitPiece[z: newZ, t: newT, from: 0, thru: newZ.length-1,
metrics: metrics,
initFree: initFree, finalFree: finalFree,
initTangent: initTangent,
finalTangent: finalTangent, useOldTValues: useOldTValues];
};
RETURN[b,err,iterations, maxDev];
};
END.