<> <> <> <> <> DIRECTORY Complex USING [VEC], Cubic USING [Bezier], LSPiece USING [FitPiece, Metrics, MetricsRec], PiecewiseFit USING [CubicProc, Data], Real USING [RoundI], Rope USING [ROPE], Seq USING [ComplexSequence, ComplexSequenceRec, JointSequenceRec, RealSequence, RealSequenceRec], Vector USING [Add, Mag, Mul, Sub, VEC]; PiecewiseFitImpl: CEDAR PROGRAM IMPORTS LSPiece, Vector, Real EXPORTS PiecewiseFit = BEGIN OPEN PiecewiseFit; Metrics: TYPE = LSPiece.Metrics; Error: PUBLIC SIGNAL[Rope.ROPE] = CODE; Default: PUBLIC PROC [data: Data] = { IF data=NIL OR data.samples=NIL OR data.samples.length=0 THEN SIGNAL Error["No samples"]; IF data.joints=NIL OR data.joints.length=0 THEN { IF data.closed THEN { data.joints _ NEW[Seq.JointSequenceRec[1]]; data.joints[0] _ [0, FALSE]; data.tangents _ NEW[Seq.ComplexSequenceRec[2]]; data.tangents[0] _ [0,0]; data.tangents[1] _ [0,0]; } ELSE { data.joints _ NEW[Seq.JointSequenceRec[2]]; data.joints[0] _ [0, FALSE]; data.joints[1] _ [data.samples.length-1, FALSE]; data.tangents _ NEW[Seq.ComplexSequenceRec[4]]; data.tangents[0] _ [0,0]; data.tangents[1] _ [0,0]; data.tangents[2] _ [0,0]; data.tangents[3] _ [0,0]; }; }; }; <> << 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, outputCubic: CubicProc] = { nPieces, start, end: INT; t: Seq.RealSequence _ NEW[Seq.RealSequenceRec[data.samples.length]]; [nPieces, start] _ Init[data]; end _ Next[data,start]; THROUGH [0..nPieces) DO b: Cubic.Bezier; err,maxd: REAL; iterations: NAT; [b,err,iterations] _ FitPieceInterp[z: data.samples, t: t, from: data.joints[start].index, thru: data.joints[end].index, metrics: metrics, initFree: FALSE, finalFree: FALSE, initTangent: data.tangents[2*start+1], finalTangent: data.tangents[2*end]]; IF outputCubic[bezier: b, sumErr: err, maxDev: maxd, iterations: iterations] THEN GOTO aborted; start _ end; end _ Next[data, end]; ENDLOOP; EXITS aborted => NULL; }; GrowSpline: PUBLIC PROC[data: Data, metrics: Metrics, outputCubic: CubicProc, hilight: CubicProc _ NIL] = { pstart,pend: NAT _ 0; t: Seq.RealSequence _ NEW[Seq.RealSequenceRec[data.samples.length]]; b: Cubic.Bezier; maxMetrics: LSPiece.Metrics _ NEW[LSPiece.MetricsRec _ [ --for final fit maxItr: metrics.maxItr, maxDev: 0, sumErr: 0, deltaT: 0]]; iterations: NAT; err, maxdev, prevErr: REAL _ 0; t1,t2: Complex.VEC; nPieces, start, last: INT; abort: SIGNAL = CODE; doFinal: PROC [start, end: NAT] = { [b,err,iterations,maxdev] _ FitPieceInterp[z: data.samples, t: t, from: data.joints[start].index, thru: data.joints[end].index, metrics: maxMetrics, initFree: FALSE, finalFree: FALSE, initTangent: t1, finalTangent: data.tangents[2*pend]]; IF outputCubic[bezier: b, sumErr: err, maxDev: maxdev, iterations: iterations] THEN SIGNAL abort; }; [nPieces, start] _ Init[data]; last _ IF data.closed THEN start ELSE data.joints.length-1; pstart _ start; --start guaranteed to be a corner if there is one DO t1 _ data.tangents[2*pstart+1]; pend _ Next[data,pstart]; DO t2 _ data.tangents[2*pend]; [b,err,iterations,maxdev] _ FitPieceInterp[z: data.samples, t: t, from: data.joints[pstart].index, thru: data.joints[pend].index, metrics: metrics, initFree: FALSE, finalFree: FALSE, initTangent: t1, finalTangent: t2]; IF hilight#NIL THEN IF hilight[bezier: b, sumErr: err, maxDev: maxdev, iterations: iterations] THEN GOTO aborted; IF maxdev>metrics.maxDev THEN { --back up if possible temp: INT _ Prev[data,pend]; IF temp#pstart THEN pend _ temp; EXIT; }; IF data.joints[pend].forced OR pend=last THEN EXIT; pend _ Next[data,pend]; ENDLOOP; <> 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; }; CostRec: TYPE = RECORD [ subtotal: REAL, bestPrev: INT _ -1, --index into cost record joint: INT _ -1, --index into joint record tIn,tOut: Complex.VEC _ [0,0] ]; CostSeq: TYPE = RECORD[element: SEQUENCE length: NAT OF CostRec]; DynSpline: PUBLIC PROC[data: Data, metrics: Metrics, penalty: REAL, trim: BOOLEAN _ TRUE, outputCubic: CubicProc, hilight: CubicProc _ NIL] = BEGIN t: Seq.RealSequence _ NEW[Seq.RealSequenceRec[data.samples.length]]; bezier: Cubic.Bezier; cost: REF CostSeq; start, end: INT _ 0; abort: ERROR = CODE; OutputPatchesUpTo: PROC [m: NAT] = { --output the best patches sumErr, maxDev: REAL; iterations: INT; IF m>0 THEN { OutputPatchesUpTo[cost[m].bestPrev]; [b: bezier, err: sumErr, iterations: iterations,maxDev: maxDev] _ FitPieceInterp[ z: data.samples, t: t, from: data.joints[cost[cost[m].bestPrev].joint].index, thru: data.joints[cost[m].joint].index, metrics: metrics, initFree: FALSE, finalFree: FALSE, initTangent: cost[cost[m].bestPrev].tOut, finalTangent: cost[m].tIn ]; IF outputCubic[bezier: bezier, sumErr: sumErr, maxDev: maxDev, iterations: iterations] THEN ERROR abort; --blasts out of recursion on user abort }; }; [, start] _ Init[data]; end _ start; cost _ NEW[CostSeq[(IF data.closed THEN data.joints.length+1 ELSE data.joints.length)]]; <> cost[0] _ [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; trimfactor: REAL _ 1.5; FitPiece: PUBLIC PROC [data: Data, from, thru: NAT, initFree, finalFree: BOOLEAN _ TRUE, metrics: Metrics, outputCubic: CubicProc] = { bezier: Cubic.Bezier; sumErr, maxDev: REAL; iterations: INT; t: Seq.RealSequence _ NEW[Seq.RealSequenceRec[data.samples.length]]; initTangent, finalTangent: Complex.VEC _ [0,0]; IF data.joints#NIL AND data.tangents#NIL THEN { fromJoint, thruJoint: INT _ -1; FOR i: NAT IN [0..data.joints.length) DO IF data.joints[i].index=from THEN fromJoint_i; IF data.joints[i].index=thru THEN thruJoint_i; ENDLOOP; IF fromJoint # -1 THEN initTangent _ data.tangents[2*fromJoint+1]; IF thruJoint # -1 THEN finalTangent _ data.tangents[2*thruJoint]; }; [b: bezier, err: sumErr, iterations: iterations, maxDev: maxDev] _ FitPieceInterp[z: data.samples, t: t, from: from, thru: thru, metrics: metrics, initFree: initFree, finalFree: finalFree, initTangent: initTangent, finalTangent: finalTangent, useOldTValues: FALSE]; [] _ outputCubic[bezier, sumErr, maxDev, iterations]; }; <> FitPieceInterp: PROCEDURE [z: Seq.ComplexSequence, t: Seq.RealSequence _ NIL, from, thru: NAT, -- fit points in range [from..thru] modulo z.length metrics: Metrics, initFree, finalFree: BOOLEAN _ FALSE, initTangent, finalTangent: Complex.VEC _ [0,0], useOldTValues: BOOLEAN _ FALSE ] RETURNS [b: Cubic.Bezier, err: REAL, iterations: NAT, maxDev: REAL] = { <> 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] _ LSPiece.FitPiece[z: z, t: t, from: from, thru: thru, metrics: metrics, initFree: initFree, finalFree: finalFree, initTangent: initTangent, finalTangent: finalTangent, useOldTValues: useOldTValues]; } ELSE { --else interpolate and then call newZ: Seq.ComplexSequence _ NEW[Seq.ComplexSequenceRec[minPoints]]; newT: Seq.RealSequence _ NEW[Seq.RealSequenceRec[minPoints]]; ninterp: INT _ minPoints-npoints; zi: NAT _ 0; modZ: PROC[index: NAT] RETURNS [NAT] = {RETURN[index MOD z.length]}; interpolate: PROC[p0,p1: Vector.VEC, npts: NAT] = { dv: Vector.VEC _ Vector.Mul[Vector.Sub[p1,p0],1.0/(npts+1)]; FOR j: NAT IN [0..npts) DO newZ[zi] _ Vector.Add[newZ[zi-1],dv]; zi _ zi+1; ENDLOOP; }; newZ[0] _ z[from]; IF npoints=2 THEN {zi _ 1; interpolate[p0: z[from], p1: z[thru], npts: ninterp]} ELSE { d: Seq.RealSequence _ NEW[Seq.RealSequenceRec[npoints-1]]; minD: REAL _ 100000000; maxD: REAL _ 0; totalD: REAL _ 0; np: NAT _ 0; i,n: NAT _ 0; adjust: BOOLEAN _ FALSE; FOR i _ from, modZ[i+1] UNTIL i=thru DO d[n] _ Vector.Mag[Vector.Sub[z[i],z[modZ[i+1]]]]; totalD _ totalD+d[n]; IF d[n]maxD THEN maxD _ d[n]; n _ n+1; ENDLOOP; FOR i IN [0..d.length) DO np _ np+Real.RoundI[(d[i]/totalD)*ninterp]; ENDLOOP; IF np#ninterp THEN adjust _ TRUE; FOR i IN [0..d.length) DO p: INTEGER _ Real.RoundI[(d[i]/totalD)*ninterp]; newZ[zi] _ z[modZ[from+i]]; zi _ zi+1; IF adjust THEN{ IF np>ninterp AND d[i]=minD THEN {p_p-1; np _ np-1}; IF np0 THEN interpolate[p0: z[modZ[from+i]], p1: z[modZ[from+i+1]], npts: p]; ENDLOOP; }; newZ[newZ.length-1] _ z[thru]; [b,err,iterations, maxDev] _ LSPiece.FitPiece[z: newZ, t: newT, from: 0, thru: newZ.length-1, metrics: metrics, initFree: initFree, finalFree: finalFree, initTangent: initTangent, finalTangent: finalTangent, useOldTValues: useOldTValues]; }; RETURN[b,err,iterations, maxDev]; }; END.