<<>> <> <> <> <> DIRECTORY FitState, FitStateUtils, DynFit, FitBasic USING [SampleHandle], Seq USING [ComplexSequence, RealSequence, RealSequenceRec, JointSequence], Vector, RealFns USING [SqRt, SinDeg, TanDeg, AlmostZero, CosDeg]; AnalyzeImpl: PROGRAM IMPORTS FitState, DynFit, FitStateUtils EXPORTS Analyze = { DeltaCurvature: PUBLIC PROC[state: FitState.Handle, tol: REAL, dynFilterTol: REAL ¬ 0] = { <0 then use Filters.Dynfilter to select samples for curvature computations.>> sa: Seq.ComplexSequence ¬ FitState.CurrentSamples[state]; closed: BOOLEAN ¬ FitState.GetClosed[state]; cpts: DynFit.Segments; IF dynFilterTol>0 THEN [segments: cpts] ¬ DynFit.FitSegments[sa, closed, dynFilterTol] ELSE { cpts ¬ NEW[DynFit.SegmentsRec[sa.length]; FOR i: NAT IN [0..cpts.length) DO cpts[i] ¬ i; ENDLOOP; }; curvature ¬ NEW[Seq.RealSeqRec[cpts.length]]; FOR i: NAT IN [0..cpts.length) DO curvature[i] ¬ Curvature[sa[cpts[wrap[i-1]]], sa[cpts[wrap[i]]],sa[cpts[wrap[i+1]]]]; ENDLOOP; FitState.ResetData[state, joints]; FOR i:NAT IN [0..curvature.length) DO FitState.SetJoint[state, cpts[i]]; ENDLOOP; }; <<>> CheckCurvature: SampleProc = { v0: Vec ¬ Sub[sa.xy,Prev[sa].xy]; v1: Vec ¬ Sub[Next[sa].xy,sa.xy]; m0: REAL ¬ ABS[sa.s-Prev[sa].s]; m1: REAL ¬ ABS[Next[sa].s-sa.s]; x: REAL ¬ Dot[v0,v1]; y: REAL ¬ Cross[v0,v1]; sa.value ¬ RealFns.ArcTanDeg[y,x]; sa.value ¬ .5*sa.value/(m0+m1); }; <> CCurveAngle: SampleProc = { v0: Vec ¬ Sub[sa.xy,Prev[sa].xy]; v1: Vec ¬ Sub[Next[sa].xy,sa.xy]; m0: REAL ¬ ABS[sa.s-Prev[sa].s]; m1: REAL ¬ ABS[Next[sa].s-sa.s]; x: REAL ¬ Dot[v0,v1]; y: REAL ¬ Cross[v0,v1]; sa.value ¬ RealFns.ArcTan[y,x]; sa.value ¬ .5*sa.value/(m0+m1); }; FindCircleCenter: PROCEDURE[p0,p1,p2:Vector.Vec] RETURNS [o:Vector.Vec,valid:BOOLEAN]= {d1,d2:Vec; d20,d21,det:REAL; x20,y20,x21,y21,x22,y22:REAL; eps:REAL¬0.001; d1¬Mul[Sub[p0,p1],2]; d2¬Mul[Sub[p1,p2],2]; x20¬p0.x*p0.x; y20¬p0.y*p0.y; x21¬p1.x*p1.x; y21¬p1.y*p1.y; x22¬p2.x*p2.x; y22¬p2.y*p2.y; d20¬x20-x21+y20-y21; d21¬x21-x22+y21-y22; det¬Det[Matrix[d1.x,d1.y,d2.x,d2.y]]; IF ABS [det] < eps THEN {RETURN[Vec[0,0],FALSE]} ELSE {o.x¬(d20*d2.y-d21*d1.y)/det; o.y¬(d1.x*d21-d2.x*d20)/det; RETURN[o,TRUE];}; }; }.