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]; }; }; }; 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 Copyright c 1985 by Xerox Corporation. All rights reserved. last edited by Maureen Stone December 13, 1984 12:39:40 pm PST last edited by Michael Plass August 30, 1982 1:42 pm Doug Wyatt, September 5, 1985 1:38:37 pm PDT 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 Κ(˜codešœ™Kšœ Οmœ1™™>Kšœ5™5K™,—K˜šΟk ˜ Kšœžœžœ˜Kšœžœ ˜Kšœžœ!˜.Kšœ žœ˜%Kšœžœ ˜Kšœžœžœ˜KšœžœX˜aKšœžœžœ˜'—K˜KšΠblœžœž˜Kšžœ˜Kšžœ ˜Kšœžœžœ˜K˜Kšœ žœ˜ Kš œžœžœžœžœ˜'šΟnœž œ˜%š žœžœžœžœžœž˜=Kšžœ˜—šžœ žœžœžœ˜1šžœ žœ˜Kšœžœ˜+Kšœžœ˜Kšœžœ˜/K˜K˜K˜—šžœ˜Kšœžœ˜+Kšœžœ,žœ˜MKšœžœ˜/K˜3K˜3K˜—K˜—K˜—Kšœ?™?Kšœc™cš  œžœžœ žœ žœ˜?Kš žœžœžœžœžœž œ˜YKš žœ žœžœžœžœ˜JKš žœžœžœžœžœ˜Pšžœžœ žœ˜Kšžœžœžœ*˜Nšžœžœ?˜[Kšžœžœ"˜-—K˜—šžœ+ž˜1Kšžœ*˜0—K™#Kšœžœ˜ šžœ žœž˜šžœžœžœž˜(Kšžœžœžœ˜1Kšžœ˜—Kšžœ˜#K˜—Kšžœžœ ˜+K˜—Kšœ ™ š  œžœžœžœžœ˜5Kšœ˜šžœž˜#Kšžœ žœžœ˜Kšžœžœ-žœ ˜I—Kšžœžœ˜K˜—K˜š  œžœžœžœžœ˜5Kšœ˜šžœ ž˜Kšžœ žœžœ˜0Kšžœžœ-žœ ˜I—Kšžœžœ˜K˜—Kš Οbœžœžœžœžœ˜?™3Kšœ žœ˜"Kšœžœ ˜Kšžœ žœžœ˜2Kš žœžœžœžœžœ˜>Kšžœ˜ K˜—š  œž œ;˜QKšœžœ˜Kšœžœ+˜DKšœ˜K˜šžœž˜K˜Kšœ žœ˜Kšœ žœ˜˜Kšœ%˜%Kšœ=˜=Kšœ˜Kšœ žœ žœ˜"KšœK˜K—šžœJ˜LKšžœžœ ˜—K˜ K˜Kšžœ˜—Kšžœ žœ˜K˜K˜—š  œž œLžœ˜kKšœ žœ˜Kšœžœ+˜DK˜šœžœΟc˜HKšœ:˜:—Kšœ žœ˜Kšœžœ˜Kšœžœ˜Kšœžœ˜Kšœžœžœ˜šœ žœžœ˜#˜AKšœ=˜=Kšœ˜Kšœ žœ žœ˜"Kšœ6˜6—šžœL˜NKšžœžœ˜—K˜—Kšœ˜Kšœžœ žœžœ˜Kšœ žœ˜!Kšœžœ˜ Kš œžœžœžœžœžœžœ ˜Dšœ žœžœžœ˜3Kšœ žœ/˜=šžœžœžœ ž˜K˜%K˜ Kšžœ˜—K˜—K˜Kšžœ žœ?˜Pšžœ˜Kšœžœ!˜:Kšœžœ ˜Kšœžœ˜Kšœžœ˜Kšœžœ˜ Kšœžœ˜ Kšœžœžœ˜šžœžœž˜'K˜1K˜Kšžœ žœ ˜Kšžœ žœ ˜K˜Kšžœ˜—Kšžœžœžœ-žœ˜NKšžœ žœ žœ˜!šžœžœž˜Kšœžœ&˜0K˜&šžœžœ˜Kšžœ žœ žœ˜4Kšžœ žœ žœ˜4Kšžœ žœ žœ’˜@—KšžœžœB˜MKšžœ˜—K˜—K˜˜]K˜K˜)K˜K˜:—K˜—Kšžœ˜!K˜K˜—Kšžœ˜—…—.AP