DIRECTORY Complex, Cubic, LSPiece, Rope USING [ROPE], Seq, Vector, PiecewiseFit, Real, RealFns; 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]; }; }; }; 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. °PiecewiseFitImpl.mesa last edited by Maureen Stone December 13, 1984 12:39:40 pm PST last edited by Michael Plass August 30, 1982 1:42 pm 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 Now we can assume well formed data. joint indexes find a good place for a joint about half-way around 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 do one more fit with the maximum number of iterations to guarentee the best cubic cost[0] holds tangents, joint index and subtotal. bestPrev is meaningless 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 penalize for over maxDev. If you're sure that you can always fit within maxDev make the penalty very large. Like LSPiece.FitPiece but will interpolate the points if necessary (need at least 6 for a fit) this makes sure there are enough points to give a good fit, then calls LSPiece.FitPiece Ê À˜J˜Jšœ™Jšœ>™>Jšœ5™5J˜šÏk ˜ Jšœ˜Jšœ˜Jšœ˜Jšœœœ˜Jšœ˜Jšœ˜J˜ Jšœ˜Jšœ˜—J˜šœœ˜Jšœ˜Jšœ˜—Jš œ ˜J˜Jšœ œ˜ Jš œœœœœ˜'šÏnœ œ˜%š œœœœœ˜=Jšœ˜—šœ œœœ˜1šœ œ˜Jšœœ˜+Jšœœ˜Jšœœ˜/J˜J˜J˜—šœ˜Jšœœ˜+Jšœœ,œ˜MJšœœ˜/J˜3J˜3J˜—J˜—J˜—Jšœ?™?Jšœc™cš žœœœ œ œ˜?Jš œœœœœ œ˜YJš œ œœœœ˜JJš œœœœœ˜Pšœœ œ˜Jšœœœ*˜Nšœœ?˜[Jšœœ"˜-—J˜—šœ+˜1Jšœ*˜0—J™#Jšœœ˜ šœ œ˜šœœœ˜(Jšœœœ˜1Jšœ˜—Jšœ˜#J˜—Jšœœ ˜+J˜—Jšœ ™ š žœœœœœ˜5Jšœ˜šœ˜#Jšœ œœ˜Jšœœ-œ ˜I—Jšœœ˜J˜—J˜š žœœœœœ˜5Jšœ˜šœ ˜Jšœ œœ˜0Jšœœ-œ ˜I—Jšœœ˜J˜—Jš Ïbœœœœœ˜?™3Jšœ œ˜"Jšœœ ˜Jšœ œœ˜2Jš œœœœœ˜>Jšœ˜ J˜—šž œ œ;˜QJšœœ˜Jšœœ+˜DJšœ˜J˜šœ˜J˜Jšœ œ˜Jšœ œ˜˜Jšœ%˜%Jšœ=˜=Jšœ˜Jšœ œ œ˜"JšœK˜K—šœJ˜LJšœœ ˜—J˜ J˜Jšœ˜—Jšœ œ˜J˜J˜—šž œ œLœ˜kJšœ œ˜Jšœœ+˜DJ˜šœœÏc˜HJšœ:˜:—Jšœ œ˜Jšœœ˜J˜Jšœœ˜Jšœœœ˜šœ œœ˜#˜AJšœ=˜=Jšœ˜Jšœ œ œ˜"Jšœ6˜6—šœL˜NJšœœ˜—J˜—Jšœ˜Jšœœ œœ˜Jšœ œ˜!Jšœœ˜ Jš œœœœœœœ ˜Dšœ œœ˜3J˜=šœœœ ˜J˜%J˜ Jšœ˜—J˜—J˜Jšœ œ?˜Pšœ˜Jšœœ!˜:Jšœœ ˜Jšœœ˜Jšœœ˜Jšœœ˜ Jšœœ˜ Jšœœœ˜šœœ˜'J˜1J˜Jšœ œ ˜Jšœ œ ˜J˜Jšœ˜—Jšœœœ-œ˜NJšœ œ œ˜!šœœ˜Jšœœ&˜0J˜&šœœ˜Jšœ œ œ˜4Jšœ œ œ˜4Jšœ œ œ ˜@—JšœœB˜MJšœ˜—J˜—J˜˜]J˜J˜)J˜J˜:—J˜—Jšœ˜!J˜J˜—Jšœ˜—…—-4?¤