<> <> <> <> <> <> <> 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]; }; }; }; <> << 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; 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: NAT _ 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; 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; <> 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: 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]; }; <> 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] _ 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; <> 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] _ 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]; <> <> <> <> <> <> <> <> <> 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, ----] _ 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! <> }; 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.