[_CDCSL_93-16_]<1>Cedar>release>PiecewiseFit>PiecewiseFitImpl.mesa

PiecewiseFitImpl.mesa

Plass, June 29, 1992 2:28 pm PDT

Wyatt, September 5, 1985 1:38:37 pm PDT

Bier, February 6, 1989 6:40:18 pm PST

Stone, September 30, 1987 6:26:26 pm PDT

Eisenman, January 11, 1988 5:46:50 pm PST

DIRECTORY

Complex, Cubic2, CubicPaths, LSPiece, PiecewiseFit, RealFns, Rope, Seq, Vector, Vectors2d;

PiecewiseFitImpl: CEDAR PROGRAM

IMPORTS Complex, CubicPaths, LSPiece, RealFns, Vector, Vectors2d

EXPORTS PiecewiseFit

= BEGIN

Cubic2Proc: TYPE = PiecewiseFit.Cubic2Proc;

Data: TYPE = PiecewiseFit.Data;

JointTanSequence: TYPE = PiecewiseFit.JointTanSequence;

TangentProc: TYPE = PiecewiseFit.TangentProc;

Metrics: TYPE = LSPiece.Metrics;

MetricsRec: TYPE = LSPiece.MetricsRec;

RealSample: TYPE = LSPiece.RealSample;

RealSampleObj: TYPE = LSPiece.RealSampleObj;

Error: PUBLIC SIGNAL[Rope.ROPE] = CODE;

VEC: TYPE = Complex.VEC;

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, outputCubic2: Cubic2Proc] = {

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: Cubic2.Bezier;

err,maxd: REAL ¬ 0;

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 outputCubic2[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, outputCubic2: Cubic2Proc, hilight: Cubic2Proc ¬ NIL] = {

pstart,pend: NAT ¬ 0;

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[data.samples.length]];

b: Cubic2.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 maxdev > metrics.maxDev THEN --relax tangent continuity

[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: [0,0], finalTangent: [0,0]];

IF outputCubic2[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

t1 ¬ data.tangents[2*pstart+1];

pend ¬ Next[data,pstart];

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;

};

leapAmount: NAT ¬ 5;

GrowSpline2: PUBLIC PROC[data: Data, realLength: NAT, metrics: Metrics, outputCubic2: Cubic2Proc, tangent: TangentProc, debug: BOOL ¬ FALSE, hilight: Cubic2Proc ¬ NIL] = { -- note: Assume not closed for now!!

pstart,pend: NAT ¬ 0;

t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[data.samples.length]];

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[realLength]];

b, thisB: Cubic2.Bezier;

maxMetrics: Metrics ¬ NEW[MetricsRec ¬ [ --for final fit

maxItr: metrics.maxItr, maxDev: 0, sumErr: 0, deltaT: 0]];

iterations, maxDevPt: 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: start, thru: end,

metrics: maxMetrics,

initFree: FALSE, finalFree: FALSE,

initTangent: t1, finalTangent: tangent[data, pend]];

IF maxdev > metrics.maxDev THEN --relax tangent continuity (Keep track of corners)

[b,err,iterations,maxdev] ← FitPieceInterp[z: data.samples, t: t,

from: start, thru: end,

metrics: maxMetrics,

initFree: FALSE, finalFree: FALSE,

initTangent: [0,0], finalTangent: [0,0]];

IF outputCubic2[bezier: b, sumErr: err, maxDev: maxdev, iterations: iterations]

THEN SIGNAL abort;

};

[nPieces, start] ← Init[data];

nPieces ¬ realLength - 1;

start ¬ 0;

last ¬ IF data.closed THEN start ELSE realLength-1;

pstart ¬ start; --start guaranteed to be a corner if there is one

lastGood: INT ¬ 0;

minNum: NAT ¬ 5;

hiPoint: NAT;

t1 ← data.tangents[2*pstart+1];

t1 ¬ tangent[data, pstart];

IF t1 # [0,0] THEN minNum ¬ minNum -1;

IF last - pstart + 1 > minNum THEN minNum ¬ minNum - 1 ;

pend ← Next[data,pstart];

pend ¬ pstart + minNum;

IF pend > last THEN pend ¬ last;

hiPoint ¬ last;

t2 ← data.tangents[2*pend];

t2 ¬ tangent[data, pend];

[thisB,err,iterations,maxdev, maxDevPt] ¬ FitPieceInterp[z: data.samples, t: t,

from: pstart, thru: pend,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: t1, finalTangent: t2];

IF hilight#NIL

THEN IF hilight[bezier: thisB, sumErr: err, maxDev: maxdev, iterations: iterations]

THEN GOTO aborted;

IF maxdev>metrics.maxDev THEN { --back up if possible

temp: INT;

temp ¬ pend - 1;

IF lastGood # 0 AND maxdev>metrics.maxDev * 4 AND lastGood + 1 < temp THEN temp ¬ lastGood + 1;

hiPoint ¬ pend - 1;

IF temp#pstart THEN pend ¬ temp;

IF pend = lastGood THEN EXIT;

}

ELSE {

b ¬ thisB;

IF pend=last OR pend = lastGood THEN EXIT;

lastGood ¬ pend;

pend ¬ pend + leapAmount;

IF pend > hiPoint THEN pend ¬ hiPoint;

};

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];

mid: NAT ¬ realLength/2;

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;

};

chunkAmount: NAT ¬ 10; -- Maybe should be based on Sample size and Density.

significant: REAL ¬ 4;

ChunkGrow: PUBLIC PROC[data: Data, realLength: NAT, metrics: Metrics, outputCubic2: Cubic2Proc, tangent: TangentProc, debug: BOOL ¬ FALSE, hilight: Cubic2Proc ¬ NIL] = { -- note: Assume not closed for now!!

pstart,pend: NAT ¬ 0;

t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[data.samples.length]];

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[realLength]];

b, lastBezier: Cubic2.Bezier;

notFirst: BOOL ¬ FALSE;

maxMetrics: Metrics ¬ NEW[MetricsRec ¬ [ --for final fit

maxItr: metrics.maxItr, maxDev: 0, sumErr: 0, deltaT: 0]];

iterations, indexOfMaxDev: NAT;

err, maxdev, prevErr: REAL ¬ 0;

t1,t2: Complex.VEC;

nPieces, start, last: INT;

abort: SIGNAL = CODE;

doFinal: PROC [start, end: NAT] = { -- if still want to do then must change code for tangent

[b,err,iterations,maxdev] ← FitPieceInterp[z: data.samples, t: t,

from: start, thru: end,

metrics: maxMetrics,

initFree: FALSE, finalFree: FALSE,

initTangent: t1, finalTangent: tangent[data, pend]];

IF maxdev > metrics.maxDev THEN --relax tangent continuity (Keep track of corners)

[b,err,iterations,maxdev] ← FitPieceInterp[z: data.samples, t: t,

from: start, thru: end,

metrics: maxMetrics,

initFree: FALSE, finalFree: FALSE,

initTangent: [0,0], finalTangent: [0,0]];

lastBezier ¬ b;

notFirst ¬ TRUE;

IF outputCubic2[bezier: b, sumErr: err, maxDev: maxdev, iterations: iterations]

THEN SIGNAL abort;

};

lastCorner: BOOL ¬ FALSE; -- OK for now since 1st sample has no tangent anyhow.

nPieces ¬ realLength - 1;

start ¬ 0;

last ¬ IF data.closed THEN start ELSE realLength-1;

pstart ¬ start; --start guaranteed to be a corner if there is one

lastGood: NAT ¬ 0;

minNum: NAT ¬ 5;

maxPoint: NAT ¬ last;

t1 ¬ tangent[data, pstart];

IF lastCorner THEN t1 ¬ [0,0]; -- bad tangent data.

IF t1 # [0,0] THEN minNum ¬ minNum -1;

IF last - pstart + 1 > minNum THEN minNum ¬ minNum - 1 ;

pend ← pstart + minNum;

pend ¬ pstart + chunkAmount;

IF pend > last THEN pend ¬ last;

t2 ¬ tangent[data, pend];

[b,err,iterations,maxdev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples, t: t,

from: pstart, thru: pend,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: t1, finalTangent: t2];

IF Corner[b, t2, TRUE] THEN {

[b,err,iterations,maxdev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples, t: t,

from: pstart, thru: pend,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: t1, finalTangent: [0,0]];

lastCorner ¬ TRUE;

}

ELSE lastCorner ¬ FALSE;

IF hilight#NIL

THEN IF hilight[bezier: b, sumErr: err, maxDev: maxdev, iterations: iterations]

THEN GOTO aborted;

IF maxdev < metrics.maxDev * significant AND pend < maxPoint THEN {

IF maxdev < metrics.maxDev THEN lastGood ¬ pend;

pend ¬ pend + chunkAmount;

IF pend > last THEN pend ¬ last;

}

ELSE { -- probably need to put curvature calc at another place?

IF maxdev>metrics.maxDev --OR (notFirst AND NOT Curvature[lastBezier, b])-- THEN {

temp: INT;

lowerSamplesNeeded, higherSamplesNeeded: NAT;

[maxDev, indexOfMaxDev] ← RealError[data.samples, pstart, pend, b]; -- more correct!

IF t1 = [0,0] THEN lowerSamplesNeeded ¬ 5 ELSE lowerSamplesNeeded ¬ 4;

IF tangent[data, last] = [0,0] THEN higherSamplesNeeded ¬ 5 ELSE higherSamplesNeeded ¬ 4;

temp ¬ indexOfMaxDev;

IF temp < lastGood - 5 THEN temp ¬ lastGood;

IF lowerSamplesNeeded + higherSamplesNeeded -1 > last - pstart + 1 THEN {

temp ← (last + pstart)/2; -- Wrong, just leave as indexOfMaxDev.

}

ELSE {

IF temp - pstart + 1 < lowerSamplesNeeded THEN temp ¬ pstart + lowerSamplesNeeded - 1

ELSE {

IF last - temp + 1 < higherSamplesNeeded THEN temp ¬ last - higherSamplesNeeded + 1;

};

IF temp >= maxPoint THEN {

temp ¬ indexOfMaxDev; -- might come up as a result of looking at whole curve in the break up process

};

maxPoint ¬ temp; -- Needs checks like recursive.

IF temp#pstart THEN pend ¬ temp;

}

ELSE EXIT;

};

ENDLOOP;

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];

trimfactor: REAL ¬ 1.5;

DynSpline: PUBLIC PROC[data: Data, metrics: Metrics, penalty: REAL, trim: BOOLEAN ¬ TRUE, outputCubic2: Cubic2Proc, hilight: Cubic2Proc ¬ NIL] =

BEGIN

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[data.samples.length]];

bezier: Cubic2.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 outputCubic2[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:REAL ¬ LAST[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;

CostRec2: TYPE = RECORD [ -- for DynSpline2

subtotal: REAL,

bestPrev: INT ¬ -1, --index into cost record

joint: INT ¬ -1, --index into joint record

tIn,tOut: Complex.VEC ¬ [0,0],

curvatureOut: REAL ¬ 999

];

CostSeq2: TYPE = RECORD[element: SEQUENCE length: NAT OF CostRec2];

curvePenalty: REAL ¬ 5;

DynSpline2: PUBLIC PROC[data: Data, realLength: NAT, metrics: Metrics, penalty: REAL, trim: BOOLEAN ¬ TRUE, outputCubic2: Cubic2Proc, tangent: TangentProc, debug: BOOL ¬ FALSE, hilight: Cubic2Proc ¬ NIL] = --Note: Assume not closed for now.

BEGIN

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[realLength]];

bezier, bestBezier: Cubic2.Bezier;

cost: REF CostSeq2;

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 outputCubic2[bezier: bezier, sumErr: sumErr, maxDev: maxDev, iterations: iterations] THEN ERROR abort; --blasts out of recursion on user abort

};

start ¬ 0;

end ¬ start;

cost ¬ NEW[CostSeq2[(IF data.closed THEN realLength+1 ELSE realLength)]];

cost[0] ¬ [subtotal: 0.0, bestPrev: -1, joint: start, tIn: tangent[data, start], tOut: tangent[data, start], curvatureOut: 999];

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:REAL ¬ LAST[INT]/2;

bestPrev: INT ¬ 0;

curveErr: REAL;

sumErr:REAL ¬ 0;

end ¬ end + 1;

{ --now work out tangents (t1, t2) for this pair of ends (prev, end)

t1,t2,bestTOut: Complex.VEC;

prev: INT ¬ end - 1;

t2 ¬ tangent[data, end];

FOR prevM: INT DECREASING IN [0..i-1] DO

t1 ¬ tangent[data, prev];

[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;

IF cost[prevM].curvatureOut # 999 THEN curveErr ¬ CurvatureErr[cost[prevM].curvatureOut, bezier];

badness ¬ penalty * 4 + maxDev+cost[prevM].subtotal + MAX[(maxDev-metrics.maxDev)*penalty,0] + curveErr * penalty * curvePenalty;

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;

bestBezier ¬ bezier;

};

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 - 1;

IF prev=end THEN EXIT; --Can't fit whole closed curve at once. should fix LSPiece2

ENDLOOP;

cost[bestPrev].tOut ¬ bestTOut;

cost[i] ¬ [subtotal: (IF data.joints[end].forced THEN 0 ELSE lowbad), bestPrev: bestPrev, joint: end, tIn: t2, curvatureOut: LSPiece.CurvatureAtPt[bestBezier, bestBezier.b3]];

};

ENDLOOP;

OutputPatchesUpTo[cost.length-1 ! abort => GOTO aborted];

EXITS aborted => NULL; --abort exit

END;

FitPiece: PUBLIC PROC [data: Data, from, thru: NAT, initFree, finalFree: BOOLEAN ¬ TRUE, metrics: Metrics, outputCubic2: Cubic2Proc] = {

bezier: Cubic2.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];

[] ¬ outputCubic2[bezier, sumErr, maxDev, iterations];

};

Like LSPiece.FitPiece but will interpolate the points if necessary (need at least 6 for a fit)

FitPieceInterp: PUBLIC PROC

[z: Seq.ComplexSequence, t: Seq.RealSequence ¬ NIL,

from, thru: NAT, -- fit points in range [from..thru] modulo z.length

metrics: Metrics,

initFree, finalFree: BOOLEAN ¬ FALSE,

initTangent, finalTangent: Complex.VEC ¬ [0,0],

useOldTValues: BOOLEAN ¬ FALSE

]

RETURNS [b: Cubic2.Bezier, err: REAL, iterations: NAT, maxDev: REAL, indexOfMaxDev: NAT] = {

this makes sure there are enough points to give a good fit, then calls LSPiece.FitPiece

npoints: NAT ¬ IF thru>from THEN thru-from+1 ELSE thru-from+z.length+1;

minPoints: NAT ¬ 6;

IF npoints>=minPoints THEN {

[b,err,iterations, maxDev, indexOfMaxDev] ¬ 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

modZ: PROC[index: NAT] RETURNS [NAT] = {RETURN[index MOD z.length]};

insert: PROC = { --at the midpoint of the maximum distance

maxD: REAL ¬ 0;

p, tail: LIST OF REF VEC ¬ NIL;

refVec: REF VEC;

p ¬ list;

FOR l: LIST OF REF VEC ¬ list, l.rest UNTIL l.rest=NIL DO

d: REAL ¬ Complex.Abs[Vector.Sub[l.first, l.rest.first]];

IF d > maxD THEN {p ¬ l; maxD ¬ d};

ENDLOOP;

tail ¬ p.rest;

refVec ¬ NEW[VEC ¬ Vector.Div[Vector.Add[p.first, p.rest.first],2]]; --interpolate

tail ¬ CONS[refVec, tail]; --insert

p.rest ¬tail;

};

list, tList: LIST OF REF VEC ¬ NIL;

minD: REAL;

index: NAT ¬ from;

pt: VEC;

newNPoints: NAT ¬ npoints;

newZ: Seq.ComplexSequence ¬ NEW[Seq.ComplexSequenceRec[minPoints]];

newT: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[minPoints]];

FOR i: NAT IN [0..npoints) DO --make a list of points

list ¬ CONS[NEW[VEC ¬ z[index]],list];

index ¬ modZ[index+1];

ENDLOOP;

UNTIL newNPoints=minPoints DO --insert interpolating points

insert[];

newNPoints ¬ newNPoints+1;

ENDLOOP;

Now we have a list minPoints long in reverse order.

FOR l: LIST OF REF VEC ¬ list, l.rest UNTIL l=NIL DO --reverse the list

tList ¬ CONS[l.first, tList];

ENDLOOP;

list ¬ tList;

FOR i: NAT IN [0..minPoints) DO --copy to the array

newZ[i] ¬ tList.first;

tList ¬ tList.rest;

ENDLOOP;

[b,err,iterations, maxDev, indexOfMaxDev] ¬ LSPiece.FitPiece[z: newZ, t: newT,

from: 0, thru: newZ.length-1,

metrics: metrics,

initFree: initFree, finalFree: finalFree,

initTangent: initTangent, finalTangent: finalTangent,

useOldTValues: useOldTValues];

The indexOfMaxDev refers to the new list. Find the closest original point to this point

pt ¬ newZ[indexOfMaxDev];

minD ¬ 10E6;

index ¬ from;

FOR i: NAT IN [0..npoints) DO

d: REAL ¬ Complex.SqrAbs[Vector.Sub[z[index], pt]];

IF d < minD THEN {indexOfMaxDev ¬ index; minD ¬ d};

index ¬ modZ[index+1];

ENDLOOP;

};

RETURN[b,err,iterations, maxDev, indexOfMaxDev];

};

FitPieceInterp2: PUBLIC PROC

[z: Seq.ComplexSequence, t: Seq.RealSequence ¬ NIL,

from, thru: NAT, -- fit points in range [from..thru] modulo z.length

metrics: Metrics,

initFree, finalFree: BOOLEAN ¬ FALSE,

initTangent, finalTangent: Complex.VEC ¬ [0,0],

useOldTValues: BOOLEAN ¬ FALSE

]

RETURNS [b: Cubic2.Bezier, err: REAL, iterations: NAT, maxDev: REAL, indexOfMaxDev: NAT] = {

This makes sure there are enough points to give a good fit, then calls LSPiece2.FitPiece

npoints: NAT ¬ IF thru>from THEN thru-from+1 ELSE thru-from+z.length+1;

minPoints: NAT ¬ 6;

IF initTangent # [0,0] THEN minPoints ¬ minPoints -1;

IF finalTangent # [0,0] THEN minPoints ¬ minPoints -1;

IF npoints>=minPoints THEN {

[b,err,iterations, maxDev, indexOfMaxDev] ¬ LSPiece.FitPiece2[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

realSampleRev: ARRAY [0..6) OF BOOLEAN; -- to keep track of inserted points

modZ: PROC[index: NAT] RETURNS [NAT] = {RETURN[index MOD z.length]};

insert: PROC = { --at the midpoint of the maximum distance

maxD: REAL ¬ 0;

p, tail: LIST OF REF VEC ¬ NIL;

refVec: REF VEC;

i, insertPoint: NAT ¬ 0;

p ¬ list;

FOR l: LIST OF REF VEC ¬ list, l.rest UNTIL l.rest=NIL DO

d: REAL ¬ Complex.Abs[Vector.Sub[l.first, l.rest.first]];

IF d > maxD THEN {p ¬ l; maxD ¬ d; insertPoint ¬ i + 1};

i ¬ i + 1;

ENDLOOP;

tail ¬ p.rest;

refVec ¬ NEW[VEC ¬ Vector.Div[Vector.Add[p.first, p.rest.first],2]]; --interpolate

tail ¬ CONS[refVec, tail]; --insert

p.rest ¬tail;

FOR i IN [insertPoint+1..minPoints) DO

realSampleRev[i] ¬ realSampleRev[i-1];

ENDLOOP;

realSampleRev[insertPoint] ¬ FALSE;

};

list, tList: LIST OF REF VEC ¬ NIL;

realSample: RealSample ¬ NEW[RealSampleObj[6]];

minD: REAL;

index: NAT ¬ from;

pt: VEC;

newNPoints: NAT ¬ npoints;

newZ: Seq.ComplexSequence ¬ NEW[Seq.ComplexSequenceRec[minPoints]];

newT: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[minPoints]];

FOR i:NAT IN [0..minPoints) DO realSampleRev[i] ¬ TRUE ENDLOOP;

FOR i: NAT IN [0..npoints) DO --make a list of points

list ¬ CONS[NEW[VEC ¬ z[index]],list];

index ¬ modZ[index+1];

ENDLOOP;

UNTIL newNPoints=minPoints DO --insert interpolating points

insert[];

newNPoints ¬ newNPoints+1;

ENDLOOP;

Now we have a list minPoints long in reverse order.

FOR l: LIST OF REF VEC ← list, l.rest UNTIL l=NIL DO --reverse the list

tList ← CONS[l.first, tList];

ENDLOOP;

list ← tList;

FOR i: NAT DECREASING IN [0..minPoints) DO --copy to the array

newZ[i] ¬ list.first;

list ¬ list.rest;

realSample[i] ¬ realSampleRev[minPoints - 1 - i];

ENDLOOP;

[b,err,iterations, maxDev, indexOfMaxDev] ¬ LSPiece.FitPiece2[z: newZ, t: newT,

from: 0, thru: newZ.length-1,

metrics: metrics,

initFree: initFree, finalFree: finalFree,

initTangent: initTangent, finalTangent: finalTangent,

useOldTValues: FALSE, -- since not saving t-values from init try, they are bogus.

someInterp: TRUE,

realSample: realSample];

The indexOfMaxDev refers to the new list. Find the closest original point to this point

pt ← newZ[indexOfMaxDev]; -- should now be able to rewrite this!!!

minD ← 10E6;

index ← from;

FOR i: NAT IN [0..npoints) DO

d: REAL ← Complex.SqrAbs[Vector.Sub[z[index], pt]];

IF d < minD THEN {indexOfMaxDev ← index; minD ← d};

index ← modZ[index+1];

ENDLOOP;

index ¬ from;

FOR i: NAT IN [1..indexOfMaxDev] DO -- maxDev should not be at an endpoint.

IF realSample[i] THEN index ¬ modZ[index + 1];

ENDLOOP;

indexOfMaxDev ¬ index;

};

RETURN[b,err,iterations, maxDev, indexOfMaxDev];

};

debug: BOOLEAN ¬ FALSE;

AdaptiveFit: PUBLIC PROC [data: Data, from, thru: NAT, initFree, finalFree: BOOLEAN ¬ TRUE, metrics: Metrics, outputCubic2: Cubic2Proc, tangent: TangentProc] ~ {

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[data.samples.length]];

testMetrics: Metrics ¬ NEW[LSPiece.MetricsRec ¬ [maxItr: 2,

maxDev: metrics.maxDev, sumErr: metrics.sumErr, deltaT: metrics.deltaT]];

aborted: SIGNAL = CODE;

fromTangent: VEC ¬ tangent[data,from];

thruTangent: VEC ¬ tangent[data,thru];

RecursiveFit: PROC[localFrom,localThru: NAT, localFromTan, localThruTan: VEC] RETURNS[doneFrom, doneThru: NAT, doneFromTan, doneThruTan: VEC] = {

b: Cubic2.Bezier;

err: REAL;

iterations: NAT;

maxDev: REAL;

indexOfMaxDev: NAT;

halfPoint: PROC RETURNS[index: NAT] = { --need to worry about closed curves

IF localThru > localFrom THEN RETURN[(localThru+ localFrom)/2]

ELSE { --This is a closed curve, and it wraps around

l: NAT ¬ data.samples.length;

d: NAT ¬ ((l-localFrom)+localThru)/2;

IF d+localFrom < l THEN RETURN [d+localFrom]

ELSE RETURN[d-l];

};

Do one iteration to see if we are at all close

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: testMetrics, --one iteration

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan, finalTangent: localThruTan,

useOldTValues: FALSE];

Use a heuristic to quit without further iteration if the fit is real bad, otherwise

try iterating further to improve the fit. Heuristic courtesy of Tony DeRose

IF maxDev> metrics.maxDev AND maxDev < metrics.maxDev*5 THEN

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan,finalTangent: localThruTan,

useOldTValues: TRUE];

Now we have the final curve fit for this try. Is is good enough?

IF maxDev > metrics.maxDev AND ABS[localFrom - localThru] > 1 THEN { --subdivide

be sure we actually do a subdivision

joint: NAT ¬ IF indexOfMaxDev=localFrom OR indexOfMaxDev=localThru THEN halfPoint[] ELSE indexOfMaxDev;

jointTan: VEC ¬ tangent[data, joint];

IF debug THEN IF outputCubic2[b, err, maxDev, iterations] THEN SIGNAL aborted;

[doneFrom, doneThru, doneFromTan, doneThruTan] ¬ RecursiveFit[localFrom,joint, localFromTan,jointTan];

[doneFrom, doneThru, doneFromTan, doneThruTan] ¬ RecursiveFit[doneThru, localThru, doneThruTan, localThruTan];

}

ELSE {

IF outputCubic2[b, err, maxDev, iterations] THEN SIGNAL aborted;

doneFrom ¬ localFrom;

doneThru ¬ localThru;

doneFromTan ¬ localFromTan;

doneThruTan ¬ localThruTan;

};

RETURN[doneFrom, doneThru, doneFromTan, doneThruTan];

};

[] ¬ RecursiveFit[from,thru, fromTangent, thruTangent ! aborted => CONTINUE];

};

AdaptiveFit2: PUBLIC PROC [data: Data, from, thru: NAT, initFree, finalFree: BOOLEAN ¬ TRUE, metrics: Metrics, outputCubic2: Cubic2Proc, tangent: TangentProc, debug: BOOL ¬ FALSE, hilight: Cubic2Proc ¬ NIL] ~ {

t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[data.samples.length]];

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[thru - from + 1]];

testMetrics: Metrics ¬ NEW[MetricsRec ¬ [maxItr: 2,

maxDev: metrics.maxDev, sumErr: metrics.sumErr, deltaT: metrics.deltaT]];

aborted: SIGNAL = CODE;

fromTangent: VEC ¬ tangent[data,from];

thruTangent: VEC ¬ tangent[data,thru];

RecursiveFit: PROC[localFrom,localThru: NAT, localFromTan, localThruTan: VEC] RETURNS[doneFrom, doneThru: NAT, doneFromTan, doneThruTan: VEC] = {

b: Cubic2.Bezier;

err: REAL;

iterations: NAT;

maxDev: REAL;

indexOfMaxDev: NAT;

redoForCorner: BOOL ¬ FALSE;

halfPoint: PROC RETURNS[index: NAT] = { --need to worry about closed curves

IF localThru > localFrom THEN RETURN[(localThru+ localFrom)/2]

ELSE { --This is a closed curve, and it wraps around

l: NAT ← data.samples.length;

l: NAT ¬ thru - from + 1;

d: NAT ¬ ((l-localFrom)+localThru)/2;

IF d+localFrom < l THEN RETURN [d+localFrom]

ELSE RETURN[d-l];

};

Do one iteration to see if we are at all close

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: testMetrics, --one iteration

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan, finalTangent: localThruTan,

useOldTValues: FALSE];

Use a heuristic to quit without further iteration if the fit is real bad, otherwise

try iterating further to improve the fit. Heuristic courtesy of Tony DeRose

IF maxDev> metrics.maxDev AND maxDev < metrics.maxDev*5 THEN

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan,finalTangent: localThruTan,

useOldTValues: TRUE];

IF maxDev <= metrics.maxDev THEN { -- Can we assume that a good fit with Tan will be a good fit without?

IF Corner[b, localThruTan, TRUE] THEN {

localThruTan ¬ [0,0];

redoForCorner ¬ TRUE;

};

IF Corner[b, localFromTan, FALSE] THEN {

localFromTan ¬ [0,0];

redoForCorner ¬ TRUE;

};

IF redoForCorner THEN {

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan,finalTangent: localThruTan,

useOldTValues: FALSE];

};

[maxDev, indexOfMaxDev] ← RealError[data.samples, localFrom, localThru, b]; -- more correct!

Now we have the final curve fit for this try. Is is good enough?

IF maxDev > metrics.maxDev AND ABS[localFrom - localThru] > 1 THEN { --subdivide

be sure we actually do a subdivision

joint, lowerSamplesNeeded, higherSamplesNeeded: NAT;

jointTan: VEC;

IF localFromTan = [0,0] THEN lowerSamplesNeeded ¬ 5 ELSE lowerSamplesNeeded ¬ 4;

IF thruTangent = [0,0] THEN higherSamplesNeeded ¬ 5 ELSE higherSamplesNeeded ¬ 4;
-- this should usu. be 5.

IF lowerSamplesNeeded + higherSamplesNeeded -1 > thru - localFrom + 1 THEN {

joint ¬ halfPoint[]; -- ? Might use indexOfMaxDev.

}

ELSE {

IF indexOfMaxDev - localFrom + 1 < lowerSamplesNeeded THEN joint ¬ localFrom + lowerSamplesNeeded - 1

ELSE {

IF thru - indexOfMaxDev + 1 < higherSamplesNeeded THEN joint ¬ thru - higherSamplesNeeded + 1

ELSE joint ¬ indexOfMaxDev;

};

IF joint >= localThru THEN joint ¬ indexOfMaxDev; -- might come up as a result of looking at whole curve in the break up process

};

jointTan ¬ tangent[data, joint];

IF hilight # NIL THEN IF hilight[b, err, maxDev, iterations] THEN SIGNAL aborted;

[doneFrom, doneThru, doneFromTan, doneThruTan] ¬ RecursiveFit[localFrom,joint, localFromTan,jointTan];

IF doneThru # thru THEN { -- fix recursion to look for bigger possible piece.

[doneFrom, doneThru, doneFromTan, doneThruTan] ¬ RecursiveFit[doneThru, thru, doneThruTan, thruTangent];

};

}

ELSE {

IF outputCubic2[b, err, maxDev, iterations] THEN SIGNAL aborted;

doneFrom ¬ localFrom;

doneThru ¬ localThru;

doneFromTan ¬ localFromTan;

doneThruTan ¬ localThruTan;

};

RETURN[doneFrom, doneThru, doneFromTan, doneThruTan];

};

[] ¬ RecursiveFit[from,thru, fromTangent, thruTangent ! aborted => CONTINUE];

};

IterFit: PUBLIC PROC [data: Data, from, thru: NAT, initFree, finalFree: BOOLEAN ¬ TRUE, metrics: Metrics, outputCubic2: Cubic2Proc, tangent: TangentProc, debug: BOOL ¬ FALSE, hilight: Cubic2Proc ¬ NIL, earlyBreak: BOOL ¬ TRUE, useMaxDev: BOOL ¬ TRUE, joints: JointTanSequence ¬ NIL] ~ {

Here joints is used to keep track of where joints were in the previous bezier traj when we are doing a clean up.

t: Seq.RealSequence ¬ NEW[Seq.RealSequenceRec[thru - from + 1]];

testMetrics: Metrics ¬ NEW[MetricsRec ¬ [maxItr: 2,

maxDev: metrics.maxDev, sumErr: metrics.sumErr, deltaT: metrics.deltaT]];

aborted: SIGNAL = CODE;

fromTangent: VEC ¬ tangent[data,from];

thruTangent: VEC ¬ tangent[data,thru];

b: Cubic2.Bezier;

err: REAL;

iterations, jointNum: NAT ¬ 0;

maxDev: REAL;

indexOfMaxDev: NAT;

redoForCorner: BOOL ¬ FALSE;

halfPoint: PROC RETURNS[index: NAT] = { --need to worry about closed curves

IF localThru > localFrom THEN RETURN[(localThru+ localFrom)/2]

ELSE { --This is a closed curve, and it wraps around

l: NAT ← data.samples.length;

l: NAT ¬ thru - from + 1;

d: NAT ¬ ((l-localFrom)+localThru)/2;

IF d+localFrom < l THEN RETURN [d+localFrom]

ELSE RETURN[d-l];

};

localFrom, localThru: NAT;

localFromTan, localThruTan, localThruTanOut: VEC;

previousThru: LIST OF NAT ¬ LIST[thru];

previousThruTan: LIST OF VEC ¬ LIST[thruTangent];

lastThru: NAT ¬ thru;

lastThruTan: VEC ¬ thruTangent;

localFrom ¬ from;

localFromTan ¬ fromTangent;

localThru ¬ thru;

localThruTan ¬ thruTangent;

localThruTanOut ¬ thruTangent;

WHILE TRUE DO

Do one iteration to see if we are at all close

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: testMetrics, --one iteration

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan, finalTangent: localThruTan,

useOldTValues: FALSE];

Use a heuristic to quit without further iteration if the fit is real bad, otherwise

try iterating further to improve the fit. Heuristic courtesy of Tony DeRose

IF maxDev> metrics.maxDev AND (maxDev < metrics.maxDev*5 OR NOT earlyBreak) THEN

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan,finalTangent: localThruTan,

useOldTValues: TRUE];

IF maxDev <= metrics.maxDev THEN { -- Can we assume that a good fit with Tan will be a good fit without?

IF Corner[b, localThruTan, TRUE] THEN {

localThruTan ¬ [0,0];

redoForCorner ¬ TRUE;

};

IF Corner[b, localFromTan, FALSE] THEN {

localFromTan ¬ [0,0];

redoForCorner ¬ TRUE;

};

IF redoForCorner THEN {

[b,err,iterations, maxDev, indexOfMaxDev] ¬ FitPieceInterp[z: data.samples,

t: t, from: localFrom, thru: localThru,

metrics: metrics,

initFree: FALSE, finalFree: FALSE,

initTangent: localFromTan,finalTangent: localThruTan,

useOldTValues: FALSE];

};

[maxDev, indexOfMaxDev] ← RealError[data.samples, localFrom, localThru, b]; -- more correct!

Now we have the final curve fit for this try. Is is good enough?

IF maxDev > metrics.maxDev AND ABS[localFrom - localThru] > 1 THEN { --subdivide

be sure we actually do a subdivision

joint, lowerSamplesNeeded, higherSamplesNeeded: NAT;

jointTan, jointTanOut: VEC;

IF localFromTan = [0,0] THEN lowerSamplesNeeded ¬ 5 ELSE lowerSamplesNeeded ¬ 4;

IF thruTangent = [0,0] THEN higherSamplesNeeded ¬ 5 ELSE higherSamplesNeeded ¬ 4;
-- this should usu. be 5.

IF lowerSamplesNeeded + higherSamplesNeeded -1 > thru - localFrom + 1 THEN {

joint ¬ halfPoint[]; -- ? Might use indexOfMaxDev.

}

ELSE {

IF indexOfMaxDev - localFrom + 1 < lowerSamplesNeeded THEN joint ¬ localFrom + lowerSamplesNeeded - 1

ELSE {

IF thru - indexOfMaxDev + 1 < higherSamplesNeeded THEN joint ¬ thru - higherSamplesNeeded + 1

ELSE joint ¬ indexOfMaxDev;

};

IF joint >= localThru THEN joint ¬ indexOfMaxDev; -- might come up as a result of looking at whole curve in the break up process

};

IF NOT useMaxDev THEN {

WHILE joints#NIL AND joints[jointNum].index <= localFrom DO

jointNum ¬ jointNum + 1;

ENDLOOP;

joint ¬ halfPoint[]; -- ? Better location

IF joints#NIL AND joints[jointNum].index > joint AND joints[jointNum].index < localThru THEN {

joint ¬ joints[jointNum].index;

jointTan ¬ joints[jointNum].tanIn;

jointTanOut ¬ joints[jointNum].tanOut;

}

ELSE {

jointTan ¬ jointTanOut ¬ tangent[data, joint];

};

}

ELSE jointTan ¬ jointTanOut ¬ tangent[data, joint];

IF hilight # NIL THEN IF hilight[b, err, maxDev, iterations] THEN SIGNAL aborted;

localFrom ¬ localFrom;

IF lastThru > localThru AND (previousThru = NIL OR lastThru < previousThru.first) THEN {

previousThru ¬ CONS[lastThru, previousThru];

previousThruTan ¬ CONS[lastThruTan, previousThruTan];

};

lastThru ¬ localThru;

lastThruTan ¬ localThruTan;

localThru ¬ joint;

localFromTan ¬ localFromTan;

localThruTan ¬ jointTan;

localThruTanOut ¬ jointTanOut;

}

ELSE {

IF outputCubic2[b, err, maxDev, iterations] THEN SIGNAL aborted;

localFrom ¬ localThru;

localFromTan ← localThruTan;

localFromTan ¬ localThruTanOut; -- only good if have just fit with a new joint.

localThru ¬ thru; -- for now always try all the way to the end if no good previous.

localThruTan ¬ localThruTanOut ¬ thruTangent;

IF Far[localThru, localFrom] AND previousThru # NIL THEN { --Hueristic so that don't always search to end.

IF previousThru.first - localFrom > 6 AND NOT (NOT useMaxDev AND joints # NIL AND (jointNum+1 = joints.length OR joints[jointNum + 1].index > previousThru.first)) THEN { -- Maybe put in a loop to look thru previousThru to make more efficient.

localThru ¬ previousThru.first;

localThruTan ¬ localThruTanOut ¬ previousThruTan.first;

lastThru ¬ localThru;

lastThruTan ¬ localThruTan;

previousThru ¬ previousThru.rest;

previousThruTan ¬ previousThruTan.rest;

}

ELSE {

previousThru ¬ LIST[thru];

previousThruTan ¬ LIST[thruTangent];

};

IF localFrom = thru THEN EXIT;

};

ENDLOOP;

};

Far: PROC [thru, from: NAT] RETURNS [isFar: BOOL ¬ FALSE] ~ {

IF thru - from > 20 THEN isFar ¬ TRUE;

};

RealError: PROC [z: Seq.ComplexSequence, from, to: NAT, b: Cubic2.Bezier] RETURNS [maxDev: REAL, indexOfMaxDev: NAT] ~ {

closest: VEC;

diff, maxDevSqr: REAL ¬ 0.0;

bezierPath: CubicPaths.Path ¬ NEW[CubicPaths.PathRec];

bezierPath.cubics ¬ NEW[CubicPaths.PathSequence[1]]; -- seems silly to have to do this

bezierPath.cubics[0] ¬ b;

FOR i: NAT IN [from..to] DO

[closest, ----] ¬ CubicPaths.ClosestPointAnalytic[z[i], bezierPath, 9999.9];

diff ¬ (z[i].x-closest.x)*(z[i].x-closest.x) + (z[i].y-closest.y)*(z[i].y-closest.y);

IF diff>maxDevSqr THEN {maxDevSqr ¬ diff; indexOfMaxDev ¬ i};

ENDLOOP;

maxDev ¬ RealFns.SqRt[maxDevSqr];

};

Corner: PROC [b: Cubic2.Bezier, tangent: VEC, checkHi: BOOL] RETURNS [BOOL ¬ FALSE] ~ {

chordLen: REAL ¬ Vectors2d.Distance[b.b0, b.b3];

tangentVector: VEC;

IF tangent = [0,0] THEN RETURN[FALSE]; -- is a corner but caller must already know.

IF checkHi THEN tangentVector ¬ Vectors2d.VectorFromPoints[tail: b.b2, head: b.b3]

ELSE tangentVector ¬ Vectors2d.VectorFromPoints[tail: b.b0, head: b.b1];

IF Vectors2d.DotProduct[tangentVector, tangent] < 0.0 THEN RETURN[TRUE]; -- if Dir fliped.

IF Vectors2d.Magnitude[tangentVector] < .02 * chordLen THEN RETURN[TRUE]; -- too short!

Note: Really do not need both tests, just use DotProduct < .02 * chordLen.

};

maxC: REAL ¬ 2.0;

Curvature: PROC [prevBezier, bezier: Cubic2.Bezier] RETURNS [BOOL ¬ TRUE] ~ {

cLast, cThis: REAL;

cLast ¬ LSPiece.ComputeCurvature[prevBezier.b1, prevBezier.b2, prevBezier.b3, FALSE];

cThis ¬ LSPiece.ComputeCurvature[bezier.b2, bezier.b1, bezier.b0, TRUE];

IF (cLast > 0.0 AND cThis < 0.0) OR (cLast < 0.0 AND cThis > 0.0) THEN RETURN[TRUE];

IF cLast/cThis > maxC OR cThis/cLast > maxC THEN RETURN[FALSE];

};

CurvatureErr: PROC [prevCurve: REAL, bezier: Cubic2.Bezier] RETURNS [REAL] ~ {

cThis: REAL ¬ LSPiece.ComputeCurvature[bezier.b2, bezier.b1, bezier.b0, TRUE];

IF (prevCurve > 0.0 AND cThis < 0.0) OR (prevCurve < 0.0 AND cThis > 0.0) THEN RETURN[1]; --??

RETURN [MAX[prevCurve/cThis, cThis/prevCurve]];

};

END.