function [pt,dudt,fofthandle] = interparc(t,px,py,varargin)
% interparc: interpolate points along a curve in 2 or more dimensions
% usage: pt = interparc(t,px,py) % a 2-d curve
% usage: pt = interparc(t,px,py,pz) % a 3-d curve
% usage: pt = interparc(t,px,py,pz,pw,...) % a 4-d or higher dimensional curve
% usage: pt = interparc(t,px,py,method) % a 2-d curve, method is specified
% usage: [pt,dudt,fofthandle] = interparc(t,px,py,...) % also returns derivatives, and a function handle
%
% Interpolates new points at any fractional point along
% the curve defined by a list of points in 2 or more
% dimensions. The curve may be defined by any sequence
% of non-replicated points.
%
% arguments: (input)
% t - vector of numbers, 0 <= t <= 1, that define
% the fractional distance along the curve to
% interpolate the curve at. t = 0 will generate
% the very first point in the point list, and
% t = 1 yields the last point in that list.
% Similarly, t = 0.5 will yield the mid-point
% on the curve in terms of arc length as the
% curve is interpolated by a parametric spline.
%
% If t is a scalar integer, at least 2, then
% it specifies the number of equally spaced
% points in arclength to be generated along
% the curve.
%
% px, py, pz, ... - vectors of length n, defining
% points along the curve. n must be at least 2.
% Exact Replicate points should not be present
% in the curve, although there is no constraint
% that the curve has replicate independent
% variables.
%
% method - (OPTIONAL) string flag - denotes the method
% used to compute the points along the curve.
%
% method may be any of 'linear', 'spline', or 'pchip',
% or any simple contraction thereof, such as 'lin',
% 'sp', or even 'p'.
%
% method == 'linear' --> Uses a linear chordal
% approximation to interpolate the curve.
% This method is the most efficient.
%
% method == 'pchip' --> Uses a parametric pchip
% approximation for the interpolation
% in arc length.
%
% method == 'spline' --> Uses a parametric spline
% approximation for the interpolation in
% arc length. Generally for a smooth curve,
% this method may be most accurate.
%
% method = 'csape' --> if available, this tool will
% allow a periodic spline fit for closed curves.
% ONLY use this method if your points should
% represent a closed curve.
%
% If the last point is NOT the same as the
% first point on the curve, then the curve
% will be forced to be periodic by this option.
% That is, the first point will be replicated
% onto the end.
%
% If csape is not present in your matlab release,
% then an error will result.
%
% DEFAULT: 'spline'
%
%
% arguments: (output)
% pt - Interpolated points at the specified fractional
% distance (in arc length) along the curve.
%
% dudt - when a second return argument is required,
% interparc will return the parametric derivatives
% (dx/dt, dy/dt, dz/dt, ...) as an array.
%
% fofthandle - a function handle, taking numbers in the interval [0,1]
% and evaluating the function at those points.
%
% Extrapolation will not be permitted by this call.
% Any values of t that lie outside of the interval [0,1]
% will be clipped to the endpoints of the curve.
%
% Example:
% % Interpolate a set of unequally spaced points around
% % the perimeter of a unit circle, generating equally
% % spaced points around the perimeter.
% theta = sort(rand(15,1))*2*pi;
% theta(end+1) = theta(1);
% px = cos(theta);
% py = sin(theta);
%
% % interpolate using parametric splines
% pt = interparc(100,px,py,'spline');
%
% % Plot the result
% plot(px,py,'r*',pt(:,1),pt(:,2),'b-o')
% axis([-1.1 1.1 -1.1 1.1])
% axis equal
% grid on
% xlabel X
% ylabel Y
% title 'Points in blue are uniform in arclength around the circle'
%
%
% Example:
% % For the previous set of points, generate exactly 6
% % points around the parametric splines, verifying
% % the uniformity of the arc length interpolant.
% pt = interparc(6,px,py,'spline');
%
% % Convert back to polar form. See that the radius
% % is indeed 1, quite accurately.
% [TH,R] = cart2pol(pt(:,1),pt(:,2))
% % TH =
% % 0.86005
% % 2.1141
% % -2.9117
% % -1.654
% % -0.39649
% % 0.86005
% % R =
% % 1
% % 0.9997
% % 0.9998
% % 0.99999
% % 1.0001
% % 1
%
% % Unwrap the polar angles, and difference them.
% diff(unwrap(TH))
% % ans =
% % 1.2541
% % 1.2573
% % 1.2577
% % 1.2575
% % 1.2565
%
% % Six points around the circle should be separated by
% % 2*pi/5 radians, if they were perfectly uniform. The
% % slight differences are due to the imperfect accuracy
% % of the parametric splines.
% 2*pi/5
% % ans =
% % 1.2566
%
%
% See also: arclength, spline, pchip, interp1
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 3/15/2010
% unpack the arguments and check for errors
if nargin < 3
error('ARCLENGTH:insufficientarguments', ...
'at least t, px, and py must be supplied')
end
t = t(:);
if (numel(t) == 1) && (t > 1) && (rem(t,1) == 0)
% t specifies the number of points to be generated
% equally spaced in arclength
t = linspace(0,1,t)';
elseif any(t < 0) || any(t > 1)
error('ARCLENGTH:impropert', ...
'All elements of t must be 0 <= t <= 1')
end
% how many points will be interpolated?
nt = numel(t);
% the number of points on the curve itself
px = px(:);
py = py(:);
n = numel(px);
% are px and py both vectors of the same length?
if ~isvector(px) || ~isvector(py) || (length(py) ~= n)
error('ARCLENGTH:improperpxorpy', ...
'px and py must be vectors of the same length')
elseif n < 2
error('ARCLENGTH:improperpxorpy', ...
'px and py must be vectors of length at least 2')
end
% compose px and py into a single array. this way,
% if more dimensions are provided, the extension
% is trivial.
pxy = [px,py];
ndim = 2;
% the default method is 'linear'
method = 'spline';
% are there any other arguments?
if nargin > 3
% there are. check the last argument. Is it a string?
if ischar(varargin{end})
method = varargin{end};
varargin(end) = [];
% method may be any of {'linear', 'pchip', 'spline', 'csape'.}
% any any simple contraction thereof.
valid = {'linear', 'pchip', 'spline', 'csape'};
[method,errstr] = validstring(method,valid);
if ~isempty(errstr)
error('INTERPARC:incorrectmethod',errstr)
end
end
% anything that remains in varargin must add
% an additional dimension on the curve/polygon
for i = 1:numel(varargin)
pz = varargin{i};
pz = pz(:);
if numel(pz) ~= n
error('ARCLENGTH:improperpxorpy', ...
'pz must be of the same size as px and py')
end
pxy = [pxy,pz]; %#ok
end
% the final number of dimensions provided
ndim = size(pxy,2);
end
% if csape, then make sure the first point is replicated at the end.
% also test to see if csape is available
if method(1) == 'c'
if exist('csape','file') == 0
error('CSAPE was requested, but you lack the necessary toolbox.')
end
p1 = pxy(1,:);
pend = pxy(end,:);
% get a tolerance on whether the first point is replicated.
if norm(p1 - pend) > 10*eps(norm(max(abs(pxy),[],1)))
% the two end points were not identical, so wrap the curve
pxy(end+1,:) = p1;
nt = nt + 1;
end
end
% preallocate the result, pt
pt = NaN(nt,ndim);
% Compute the chordal (linear) arclength
% of each segment. This will be needed for
% any of the methods.
chordlen = sqrt(sum(diff(pxy,[],1).^2,2));
% Normalize the arclengths to a unit total
chordlen = chordlen/sum(chordlen);
% cumulative arclength
cumarc = [0;cumsum(chordlen)];
% The linear interpolant is trivial. do it as a special case
if method(1) == 'l'
% The linear method.
% which interval did each point fall in, in
% terms of t?
[junk,tbins] = histc(t,cumarc); %#ok
% catch any problems at the ends
tbins((tbins <= 0) | (t <= 0)) = 1;
tbins((tbins >= n) | (t >= 1)) = n - 1;
% interpolate
s = (t - cumarc(tbins))./chordlen(tbins);
% be nice, and allow the code to work on older releases
% that don't have bsxfun
pt = pxy(tbins,:) + (pxy(tbins+1,:) - pxy(tbins,:)).*repmat(s,1,ndim);
% do we need to compute derivatives here?
if nargout > 1
dudt = (pxy(tbins+1,:) - pxy(tbins,:))./repmat(chordlen(tbins),1,ndim);
end
% do we need to create the spline as a piecewise linear function?
if nargout > 2
spl = cell(1,ndim);
for i = 1:ndim
coefs = [diff(pxy(:,i))./diff(cumarc),pxy(1:(end-1),i)];
spl{i} = mkpp(cumarc.',coefs);
end
%create a function handle for evaluation, passing in the splines
fofthandle = @(t) foft(t,spl);
end
% we are done at this point
return
end
% If we drop down to here, we have either a spline
% or csape or pchip interpolant to work with.
% compute parametric splines
spl = cell(1,ndim);
spld = spl;
diffarray = [3 0 0;0 2 0;0 0 1;0 0 0];
for i = 1:ndim
switch method
case 'pchip'
spl{i} = pchip(cumarc,pxy(:,i));
case 'spline'
spl{i} = spline(cumarc,pxy(:,i));
nc = numel(spl{i}.coefs);
if nc < 4
% just pretend it has cubic segments
spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
spl{i}.order = 4;
end
case 'csape'
% csape was specified, so the curve is presumed closed,
% therefore periodic
spl{i} = csape(cumarc,pxy(:,i),'periodic');
nc = numel(spl{i}.coefs);
if nc < 4
% just pretend it has cubic segments
spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
spl{i}.order = 4;
end
end
% and now differentiate them
xp = spl{i};
xp.coefs = xp.coefs*diffarray;
xp.order = 3;
spld{i} = xp;
end
% catch the case where there were exactly three points
% in the curve, and spline was used to generate the
% interpolant. In this case, spline creates a curve with
% only one piece, not two.
if (numel(cumarc) == 3) && (method(1) == 's')
cumarc = spl{1}.breaks;
n = numel(cumarc);
chordlen = sum(chordlen);
end
% Generate the total arclength along the curve
% by integrating each segment and summing the
% results. The integration scheme does its job
% using an ode solver.
% polyarray here contains the derivative polynomials
% for each spline in a given segment
polyarray = zeros(ndim,3);
seglen = zeros(n-1,1);
% options for ode45
opts = odeset('reltol',1.e-9);
for i = 1:spl{1}.pieces
% extract polynomials for the derivatives
for j = 1:ndim
polyarray(j,:) = spld{j}.coefs(i,:);
end
% integrate the arclength for the i'th segment
% using ode45 for the integral. I could have
% done this part with quad too, but then it
% would not have been perfectly (numerically)
% consistent with the next operation in this tool.
[tout,yout] = ode45(@(t,y) segkernel(t,y),[0,chordlen(i)],0,opts); %#ok
seglen(i) = yout(end);
end
% and normalize the segments to have unit total length
totalsplinelength = sum(seglen);
cumseglen = [0;cumsum(seglen)];
% which interval did each point fall into, in
% terms of t, but relative to the cumulative
% arc lengths along the parametric spline?
[junk,tbins] = histc(t*totalsplinelength,cumseglen); %#ok
% catch any problems at the ends
tbins((tbins <= 0) | (t <= 0)) = 1;
tbins((tbins >= n) | (t >= 1)) = n - 1;
% Do the fractional integration within each segment
% for the interpolated points. t is the parameter
% used to define the splines. It is defined in terms
% of a linear chordal arclength. This works nicely when
% a linear piecewise interpolant was used. However,
% what is asked for is an arclength interpolation
% in terms of arclength of the spline itself. Call s
% the arclength traveled along the spline.
s = totalsplinelength*t;
% the ode45 options will now include an events property
% so we can catch zero crossings.
opts = odeset('reltol',1.e-9,'events',@ode_events);
ti = t;
for i = 1:nt
% si is the piece of arc length that we will look
% for in this spline segment.
si = s(i) - cumseglen(tbins(i));
% extract polynomials for the derivatives
% in the interval the point lies in
for j = 1:ndim
polyarray(j,:) = spld{j}.coefs(tbins(i),:);
end
% we need to integrate in t, until the integral
% crosses the specified value of si. Because we
% have defined totalsplinelength, the lengths will
% be normalized at this point to a unit length.
%
% Start the ode solver at -si, so we will just
% look for an event where y crosses zero.
[tout,yout,te,ye] = ode45(@(t,y) segkernel(t,y),[0,chordlen(tbins(i))],-si,opts); %#ok
% we only need that point where a zero crossing occurred
% if no crossing was found, then we can look at each end.
if ~isempty(te)
ti(i) = te(1) + cumarc(tbins(i));
else
% a crossing must have happened at the very
% beginning or the end, and the ode solver
% missed it, not trapping that event.
if abs(yout(1)) < abs(yout(end))
% the event must have been at the start.
ti(i) = tout(1) + cumarc(tbins(i));
else
% the event must have been at the end.
ti(i) = tout(end) + cumarc(tbins(i));
end
end
end
% Interpolate the parametric splines at ti to get
% our interpolated value.
for L = 1:ndim
pt(:,L) = ppval(spl{L},ti);
end
% do we need to compute first derivatives here at each point?
if nargout > 1
dudt = zeros(nt,ndim);
for L = 1:ndim
dudt(:,L) = ppval(spld{L},ti);
end
end
% create a function handle for evaluation, passing in the splines
if nargout > 2
fofthandle = @(t) foft(t,spl);
end
% ===============================================
% nested function for the integration kernel
% ===============================================
function val = segkernel(t,y) %#ok
% sqrt((dx/dt)^2 + (dy/dt)^2 + ...)
val = zeros(size(t));
for k = 1:ndim
val = val + polyval(polyarray(k,:),t).^2;
end
val = sqrt(val);
end % function segkernel
% ===============================================
% nested function for ode45 integration events
% ===============================================
function [value,isterminal,direction] = ode_events(t,y) %#ok
% ode event trap, looking for zero crossings of y.
value = y;
isterminal = ones(size(y));
direction = ones(size(y));
end % function ode_events
end % mainline - interparc
% ===============================================
% end mainline - interparc
% ===============================================
% begin subfunctions
% ===============================================
% ===============================================
% subfunction for evaluation at any point externally
% ===============================================
function f_t = foft(t,spl)
% tool allowing the user to evaluate the interpolant at any given point for any values t in [0,1]
pdim = numel(spl);
f_t = zeros(numel(t),pdim);
% convert t to a column vector, clipping it to [0,1] as we do.
t = max(0,min(1,t(:)));
% just loop over the splines in the cell array of splines
for i = 1:pdim
f_t(:,i) = ppval(spl{i},t);
end
end % function foft
function [str,errorclass] = validstring(arg,valid)
% validstring: compares a string against a set of valid options
% usage: [str,errorclass] = validstring(arg,valid)
%
% If a direct hit, or any unambiguous shortening is found, that
% string is returned. Capitalization is ignored.
%
% arguments: (input)
% arg - character string, to be tested against a list
% of valid choices. Capitalization is ignored.
%
% valid - cellstring array of alternative choices
%
% Arguments: (output)
% str - string - resulting choice resolved from the
% list of valid arguments. If no unambiguous
% choice can be resolved, then str will be empty.
%
% errorclass - string - A string argument that explains
% the error. It will be one of the following
% possibilities:
%
% '' --> No error. An unambiguous match for arg
% was found among the choices.
%
% 'No match found' --> No match was found among
% the choices provided in valid.
%
% 'Ambiguous argument' --> At least two ambiguous
% matches were found among those provided
% in valid.
%
%
% Example:
% valid = {'off' 'on' 'The sky is falling'}
%
%
% See also: parse_pv_pairs, strmatch, strcmpi
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 3/25/2010
ind = find(strncmpi(lower(arg),valid,numel(arg)));
if isempty(ind)
% No hit found
errorclass = 'No match found';
str = '';
elseif (length(ind) > 1)
% Ambiguous arg, hitting more than one of the valid options
errorclass = 'Ambiguous argument';
str = '';
return
else
errorclass = '';
str = valid{ind};
end
end % function validstring