<<>> <> <> <> <> <> <> <> 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]; }; }; }; <> << 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"]; <> 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]; }; <> 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] = { <> 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 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; <> IF pend=pstart THEN { mid: NAT ¬ SelectMidJoint[data, pstart]; doFinal[pstart, mid ! abort => GOTO aborted]; pstart ¬ mid; }; <> 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[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,>> <> <> <> <> < metrics.maxDev THEN --relax tangent continuity (Keep track of corners)>> <<[b,err,iterations,maxdev] _ FitPieceInterp[z: data.samples, t: t,>> <> <> <> <> 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: INT ¬ 0; minNum: NAT ¬ 5; hiPoint: NAT; <> t1 ¬ tangent[data, pstart]; IF t1 # [0,0] THEN minNum ¬ minNum -1; IF last - pstart + 1 > minNum THEN minNum ¬ minNum - 1 ; <> pend ¬ pstart + minNum; IF pend > last THEN pend ¬ last; hiPoint ¬ last; DO <> 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; <> IF pend=pstart THEN { <> mid: NAT ¬ realLength/2; doFinal[pstart, mid ! abort => GOTO aborted]; pstart ¬ mid; }; <> 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[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,>> <> <> <> <> < metrics.maxDev THEN --relax tangent continuity (Keep track of corners)>> <<[b,err,iterations,maxdev] _ FitPieceInterp[z: data.samples, t: t,>> <> <> <> <> 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 + 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 { <> } 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; <> 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] ¬ [subtotal: 0.0, bestPrev: -1, joint: start, tIn: data.tangents[2*start], tOut: data.tangents[2*start+1]]; <> <> 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]; <> 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]; <> <> 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; <> 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]; }; <> 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] = { <> 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; <> 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]; <> 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] = { <> 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]]; <> index: NAT ¬ from; <> 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; <> <> <> <> <> 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]; <> <> <> <> <> <> <> <> <> 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]; }; }; <> [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]; <> <> 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 AND ABS[localFrom - localThru] > 1 THEN { --subdivide <> 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[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 ¬ thru - from + 1; d: NAT ¬ ((l-localFrom)+localThru)/2; IF d+localFrom < l THEN RETURN [d+localFrom] ELSE RETURN[d-l]; }; }; <> [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]; <> <> 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!>> <> IF maxDev > metrics.maxDev AND ABS[localFrom - localThru] > 1 THEN { --subdivide <> 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] ~ { <> 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 ¬ 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 <> [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]; <> <> 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!>> <> IF maxDev > metrics.maxDev AND ABS[localFrom - localThru] > 1 THEN { --subdivide <> 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 ¬ 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! <> }; 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.