% find orthogonal intersection between a point and the worm midline axis % specified by the user-supplied axis and the splined axis . % return DL, where DL(1) is the length along the spline and DL(2) is the % length orthogonal to the spline. function DX = findAxisIntersection (x, ax, ax_dr, ax_length, ax_normal, pp, pp_dr, pp_length, pp_normal, draw) % first find orthogonal interestion with user-defined axis, use % that to find intersection with spline later [lambda, seg, L] = findOrthogonalIntersection(x, 1:size(ax,2)-1, ax(:,1:end-1), ax_dr, ax_normal, ax_length,0); if seg>0 if draw % plot intersection with axis segment n2=[ax_normal(2,seg); -ax_normal(1,seg)]; b=x-lambda(2)*n2; plot([x(1) b(1)], [x(2) b(2)], '-y'); end % find all segments in spline with certain range of axis length L_range=25; r=find(pp_length > L-L_range & pp_length < L+L_range); % find intersection with the spline in the above spline segments [lambda, seg, L] = findOrthogonalIntersection(x, r, pp(:,r), pp_dr(r), pp_normal(:,r), pp_length(r),0); % plot intersection with spline if seg>0 && draw x1=pp(1:2,seg); a=x1-lambda(1)*pp_normal(:,seg); n2=[pp_normal(2,seg); -pp_normal(1,seg)]; b=x-lambda(2)*n2; plot([x(1) b(1)], [x(2) b(2)], '-r'); end end if seg>0 DX=[L; -lambda(2)]; else DX=[-1; -1]; end end % find orthogonal intersection of point x with line , with segment % length and normal and length . function [lambda_min, seg, L] = findOrthogonalIntersection(x, s, p, ds, n, l, draw) q=[]; L=[]; lambda_min=[]; seg=[]; L_MIN=666e6; seg=-1; for i=1:size(p,2) % for each segment of axis find orthogonal intersection x1=p(:,i); A=[-n(1,i) n(2,i); -n(2,i) -n(1,i)]; lambda=A\(x-x1); if draw==1 x1=p(:,i); a=x1-lambda(1)*n(:,i); n2=[n(2,i); -n(1,i)]; b=x-lambda(2)*n2; plot([x1(1) a(1)], [x1(2) a(2)], '-w'); plot([x(1) b(1)], [x(2) b(2)], '-r'); end % if intersection occurs within range of current segment % 1.5 is bit arbitrary but for small segments one can't always find % intersection exactly within segment... if lambda(1)>0 && lambda(1)<1.5*ds(i) % check whether the distance from x to the intersection is % the current minimum if lambda(1)1 L=l(i-1)+lambda(1); %PROPER else seg=-1; end end end end end end