<> <> <> 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];}; }; }.