PiecewiseFitImpl.mesa
Copyright © 1985 by Xerox Corporation. All rights reserved.
Plass, August 30, 1982 1:42 pm
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, CubicPathsExtras, LSPiece, LSPieceExtras, PiecewiseFit, PiecewiseFitExtras, RealFns, Rope, Seq, Vector, Vectors2d;
PiecewiseFitImpl: CEDAR PROGRAM
IMPORTS Complex, CubicPathsExtras, LSPiece, LSPieceExtras, RealFns, Vector, Vectors2d
EXPORTS PiecewiseFit, PiecewiseFitExtras
= BEGIN OPEN PiecewiseFit;
Cubic2Proc: TYPE = PiecewiseFit.Cubic2Proc;
Data: TYPE = PiecewiseFit.Data;
JointTanSequence: TYPE = PiecewiseFitExtras.JointTanSequence;
TangentProc: TYPE = PiecewiseFit.TangentProc;
Metrics: TYPE = LSPiece.Metrics;
MetricsRec: TYPE = LSPiece.MetricsRec;
RealSample: TYPE = LSPieceExtras.RealSample;
RealSampleObj: TYPE = LSPieceExtras.RealSampleObj;
-- Global Variables (for Eisenman's additions)
realSample: RealSample ← NEW[RealSampleObj[6]]; -- global for now
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;
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
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;
};
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
DO
lastGood: NAT ← 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;
DO
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;
IF lastGood # 0 AND maxdev>metrics.maxDev * 4 THEN temp ← MIN[lastGood + 1, pend - 1] -- too far H.
ELSE {
temp ← pend -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
DO
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;
DO
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: LSPieceExtras.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] ← LSPieceExtras.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;
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] ← LSPieceExtras.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, ----] ← CubicPathsExtras.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 ← LSPieceExtras.ComputeCurvature[prevBezier.b1, prevBezier.b2, prevBezier.b3, FALSE];
cThis ← LSPieceExtras.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 ← LSPieceExtras.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.