interparc (Copyright (c) 2012, John D'Errico): 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
0001 function [pt,dudt,fofthandle] = interparc(t,px,py,varargin) 0002 % interparc (Copyright (c) 2012, John D'Errico): interpolate points along a curve in 2 or more dimensions 0003 % usage: pt = interparc(t,px,py) % a 2-d curve 0004 % usage: pt = interparc(t,px,py,pz) % a 3-d curve 0005 % usage: pt = interparc(t,px,py,pz,pw,...) % a 4-d or higher dimensional curve 0006 % usage: pt = interparc(t,px,py,method) % a 2-d curve, method is specified 0007 % usage: [pt,dudt,fofthandle] = interparc(t,px,py,...) % also returns derivatives, and a function handle 0008 % 0009 % Interpolates new points at any fractional point along 0010 % the curve defined by a list of points in 2 or more 0011 % dimensions. The curve may be defined by any sequence 0012 % of non-replicated points. 0013 % 0014 % arguments: (input) 0015 % t - vector of numbers, 0 <= t <= 1, that define 0016 % the fractional distance along the curve to 0017 % interpolate the curve at. t = 0 will generate 0018 % the very first point in the point list, and 0019 % t = 1 yields the last point in that list. 0020 % Similarly, t = 0.5 will yield the mid-point 0021 % on the curve in terms of arc length as the 0022 % curve is interpolated by a parametric spline. 0023 % 0024 % If t is a scalar integer, at least 2, then 0025 % it specifies the number of equally spaced 0026 % points in arclength to be generated along 0027 % the curve. 0028 % 0029 % px, py, pz, ... - vectors of length n, defining 0030 % points along the curve. n must be at least 2. 0031 % Exact Replicate points should not be present 0032 % in the curve, although there is no constraint 0033 % that the curve has replicate independent 0034 % variables. 0035 % 0036 % method - (OPTIONAL) string flag - denotes the method 0037 % used to compute the points along the curve. 0038 % 0039 % method may be any of 'linear', 'spline', or 'pchip', 0040 % or any simple contraction thereof, such as 'lin', 0041 % 'sp', or even 'p'. 0042 % 0043 % method == 'linear' --> Uses a linear chordal 0044 % approximation to interpolate the curve. 0045 % This method is the most efficient. 0046 % 0047 % method == 'pchip' --> Uses a parametric pchip 0048 % approximation for the interpolation 0049 % in arc length. 0050 % 0051 % method == 'spline' --> Uses a parametric spline 0052 % approximation for the interpolation in 0053 % arc length. Generally for a smooth curve, 0054 % this method may be most accurate. 0055 % 0056 % method = 'csape' --> if available, this tool will 0057 % allow a periodic spline fit for closed curves. 0058 % ONLY use this method if your points should 0059 % represent a closed curve. 0060 % 0061 % If the last point is NOT the same as the 0062 % first point on the curve, then the curve 0063 % will be forced to be periodic by this option. 0064 % That is, the first point will be replicated 0065 % onto the end. 0066 % 0067 % If csape is not present in your matlab release, 0068 % then an error will result. 0069 % 0070 % DEFAULT: 'spline' 0071 % 0072 % 0073 % arguments: (output) 0074 % pt - Interpolated points at the specified fractional 0075 % distance (in arc length) along the curve. 0076 % 0077 % dudt - when a second return argument is required, 0078 % interparc will return the parametric derivatives 0079 % (dx/dt, dy/dt, dz/dt, ...) as an array. 0080 % 0081 % fofthandle - a function handle, taking numbers in the interval [0,1] 0082 % and evaluating the function at those points. 0083 % 0084 % Extrapolation will not be permitted by this call. 0085 % Any values of t that lie outside of the interval [0,1] 0086 % will be clipped to the endpoints of the curve. 0087 % 0088 % Example: 0089 % % Interpolate a set of unequally spaced points around 0090 % % the perimeter of a unit circle, generating equally 0091 % % spaced points around the perimeter. 0092 % theta = sort(rand(15,1))*2*pi; 0093 % theta(end+1) = theta(1); 0094 % px = cos(theta); 0095 % py = sin(theta); 0096 % 0097 % % interpolate using parametric splines 0098 % pt = interparc(100,px,py,'spline'); 0099 % 0100 % % Plot the result 0101 % plot(px,py,'r*',pt(:,1),pt(:,2),'b-o') 0102 % axis([-1.1 1.1 -1.1 1.1]) 0103 % axis equal 0104 % grid on 0105 % xlabel X 0106 % ylabel Y 0107 % title 'Points in blue are uniform in arclength around the circle' 0108 % 0109 % 0110 % Example: 0111 % % For the previous set of points, generate exactly 6 0112 % % points around the parametric splines, verifying 0113 % % the uniformity of the arc length interpolant. 0114 % pt = interparc(6,px,py,'spline'); 0115 % 0116 % % Convert back to polar form. See that the radius 0117 % % is indeed 1, quite accurately. 0118 % [TH,R] = cart2pol(pt(:,1),pt(:,2)) 0119 % % TH = 0120 % % 0.86005 0121 % % 2.1141 0122 % % -2.9117 0123 % % -1.654 0124 % % -0.39649 0125 % % 0.86005 0126 % % R = 0127 % % 1 0128 % % 0.9997 0129 % % 0.9998 0130 % % 0.99999 0131 % % 1.0001 0132 % % 1 0133 % 0134 % % Unwrap the polar angles, and difference them. 0135 % diff(unwrap(TH)) 0136 % % ans = 0137 % % 1.2541 0138 % % 1.2573 0139 % % 1.2577 0140 % % 1.2575 0141 % % 1.2565 0142 % 0143 % % Six points around the circle should be separated by 0144 % % 2*pi/5 radians, if they were perfectly uniform. The 0145 % % slight differences are due to the imperfect accuracy 0146 % % of the parametric splines. 0147 % 2*pi/5 0148 % % ans = 0149 % % 1.2566 0150 % 0151 % 0152 % See also: arclength, spline, pchip, interp1 0153 % 0154 % Author: John D'Errico 0155 % e-mail: woodchips@rochester.rr.com 0156 % Release: 1.0 0157 % Release date: 3/15/2010 0158 0159 % unpack the arguments and check for errors 0160 if nargin < 3 0161 error('ARCLENGTH:insufficientarguments', ... 0162 'at least t, px, and py must be supplied') 0163 end 0164 0165 t = t(:); 0166 if (numel(t) == 1) && (t > 1) && (rem(t,1) == 0) 0167 % t specifies the number of points to be generated 0168 % equally spaced in arclength 0169 t = linspace(0,1,t)'; 0170 elseif any(t < 0) || any(t > 1) 0171 error('ARCLENGTH:impropert', ... 0172 'All elements of t must be 0 <= t <= 1') 0173 end 0174 0175 % how many points will be interpolated? 0176 nt = numel(t); 0177 0178 % the number of points on the curve itself 0179 px = px(:); 0180 py = py(:); 0181 n = numel(px); 0182 0183 % are px and py both vectors of the same length? 0184 if ~isvector(px) || ~isvector(py) || (length(py) ~= n) 0185 error('ARCLENGTH:improperpxorpy', ... 0186 'px and py must be vectors of the same length') 0187 elseif n < 2 0188 error('ARCLENGTH:improperpxorpy', ... 0189 'px and py must be vectors of length at least 2') 0190 end 0191 0192 % compose px and py into a single array. this way, 0193 % if more dimensions are provided, the extension 0194 % is trivial. 0195 pxy = [px,py]; 0196 ndim = 2; 0197 0198 % the default method is 'linear' 0199 method = 'spline'; 0200 0201 % are there any other arguments? 0202 if nargin > 3 0203 % there are. check the last argument. Is it a string? 0204 if ischar(varargin{end}) 0205 method = varargin{end}; 0206 varargin(end) = []; 0207 0208 % method may be any of {'linear', 'pchip', 'spline', 'csape'.} 0209 % any any simple contraction thereof. 0210 valid = {'linear', 'pchip', 'spline', 'csape'}; 0211 [method,errstr] = validstring(method,valid); 0212 if ~isempty(errstr) 0213 error('INTERPARC:incorrectmethod',errstr) 0214 end 0215 end 0216 0217 % anything that remains in varargin must add 0218 % an additional dimension on the curve/polygon 0219 for i = 1:numel(varargin) 0220 pz = varargin{i}; 0221 pz = pz(:); 0222 if numel(pz) ~= n 0223 error('ARCLENGTH:improperpxorpy', ... 0224 'pz must be of the same size as px and py') 0225 end 0226 pxy = [pxy,pz]; %#ok 0227 end 0228 0229 % the final number of dimensions provided 0230 ndim = size(pxy,2); 0231 end 0232 0233 % if csape, then make sure the first point is replicated at the end. 0234 % also test to see if csape is available 0235 if method(1) == 'c' 0236 if exist('csape','file') == 0 0237 error('CSAPE was requested, but you lack the necessary toolbox.') 0238 end 0239 0240 p1 = pxy(1,:); 0241 pend = pxy(end,:); 0242 0243 % get a tolerance on whether the first point is replicated. 0244 if norm(p1 - pend) > 10*eps(norm(max(abs(pxy),[],1))) 0245 % the two end points were not identical, so wrap the curve 0246 pxy(end+1,:) = p1; 0247 nt = nt + 1; 0248 end 0249 end 0250 0251 % preallocate the result, pt 0252 pt = NaN(nt,ndim); 0253 0254 % Compute the chordal (linear) arclength 0255 % of each segment. This will be needed for 0256 % any of the methods. 0257 chordlen = sqrt(sum(diff(pxy,[],1).^2,2)); 0258 0259 % Normalize the arclengths to a unit total 0260 chordlen = chordlen/sum(chordlen); 0261 0262 % cumulative arclength 0263 cumarc = [0;cumsum(chordlen)]; 0264 0265 % The linear interpolant is trivial. do it as a special case 0266 if method(1) == 'l' 0267 % The linear method. 0268 0269 % which interval did each point fall in, in 0270 % terms of t? 0271 [junk,tbins] = histc(t,cumarc); %#ok 0272 0273 % catch any problems at the ends 0274 tbins((tbins <= 0) | (t <= 0)) = 1; 0275 tbins((tbins >= n) | (t >= 1)) = n - 1; 0276 0277 % interpolate 0278 s = (t - cumarc(tbins))./chordlen(tbins); 0279 % be nice, and allow the code to work on older releases 0280 % that don't have bsxfun 0281 pt = pxy(tbins,:) + (pxy(tbins+1,:) - pxy(tbins,:)).*repmat(s,1,ndim); 0282 0283 % do we need to compute derivatives here? 0284 if nargout > 1 0285 dudt = (pxy(tbins+1,:) - pxy(tbins,:))./repmat(chordlen(tbins),1,ndim); 0286 end 0287 0288 % do we need to create the spline as a piecewise linear function? 0289 if nargout > 2 0290 spl = cell(1,ndim); 0291 for i = 1:ndim 0292 coefs = [diff(pxy(:,i))./diff(cumarc),pxy(1:(end-1),i)]; 0293 spl{i} = mkpp(cumarc.',coefs); 0294 end 0295 0296 %create a function handle for evaluation, passing in the splines 0297 fofthandle = @(t) foft(t,spl); 0298 end 0299 0300 % we are done at this point 0301 return 0302 end 0303 0304 % If we drop down to here, we have either a spline 0305 % or csape or pchip interpolant to work with. 0306 0307 % compute parametric splines 0308 spl = cell(1,ndim); 0309 spld = spl; 0310 diffarray = [3 0 0;0 2 0;0 0 1;0 0 0]; 0311 for i = 1:ndim 0312 switch method 0313 case 'pchip' 0314 spl{i} = pchip(cumarc,pxy(:,i)); 0315 case 'spline' 0316 spl{i} = spline(cumarc,pxy(:,i)); 0317 nc = numel(spl{i}.coefs); 0318 if nc < 4 0319 % just pretend it has cubic segments 0320 spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs]; 0321 spl{i}.order = 4; 0322 end 0323 case 'csape' 0324 % csape was specified, so the curve is presumed closed, 0325 % therefore periodic 0326 spl{i} = csape(cumarc,pxy(:,i),'periodic'); 0327 nc = numel(spl{i}.coefs); 0328 if nc < 4 0329 % just pretend it has cubic segments 0330 spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs]; 0331 spl{i}.order = 4; 0332 end 0333 end 0334 0335 % and now differentiate them 0336 xp = spl{i}; 0337 xp.coefs = xp.coefs*diffarray; 0338 xp.order = 3; 0339 spld{i} = xp; 0340 end 0341 0342 % catch the case where there were exactly three points 0343 % in the curve, and spline was used to generate the 0344 % interpolant. In this case, spline creates a curve with 0345 % only one piece, not two. 0346 if (numel(cumarc) == 3) && (method(1) == 's') 0347 cumarc = spl{1}.breaks; 0348 n = numel(cumarc); 0349 chordlen = sum(chordlen); 0350 end 0351 0352 % Generate the total arclength along the curve 0353 % by integrating each segment and summing the 0354 % results. The integration scheme does its job 0355 % using an ode solver. 0356 0357 % polyarray here contains the derivative polynomials 0358 % for each spline in a given segment 0359 polyarray = zeros(ndim,3); 0360 seglen = zeros(n-1,1); 0361 0362 % options for ode45 0363 opts = odeset('reltol',1.e-9); 0364 for i = 1:spl{1}.pieces 0365 % extract polynomials for the derivatives 0366 for j = 1:ndim 0367 polyarray(j,:) = spld{j}.coefs(i,:); 0368 end 0369 0370 % integrate the arclength for the i'th segment 0371 % using ode45 for the integral. I could have 0372 % done this part with quad too, but then it 0373 % would not have been perfectly (numerically) 0374 % consistent with the next operation in this tool. 0375 [tout,yout] = ode45(@(t,y) segkernel(t,y),[0,chordlen(i)],0,opts); %#ok 0376 seglen(i) = yout(end); 0377 end 0378 0379 % and normalize the segments to have unit total length 0380 totalsplinelength = sum(seglen); 0381 cumseglen = [0;cumsum(seglen)]; 0382 0383 % which interval did each point fall into, in 0384 % terms of t, but relative to the cumulative 0385 % arc lengths along the parametric spline? 0386 [junk,tbins] = histc(t*totalsplinelength,cumseglen); %#ok 0387 0388 % catch any problems at the ends 0389 tbins((tbins <= 0) | (t <= 0)) = 1; 0390 tbins((tbins >= n) | (t >= 1)) = n - 1; 0391 0392 % Do the fractional integration within each segment 0393 % for the interpolated points. t is the parameter 0394 % used to define the splines. It is defined in terms 0395 % of a linear chordal arclength. This works nicely when 0396 % a linear piecewise interpolant was used. However, 0397 % what is asked for is an arclength interpolation 0398 % in terms of arclength of the spline itself. Call s 0399 % the arclength traveled along the spline. 0400 s = totalsplinelength*t; 0401 0402 % the ode45 options will now include an events property 0403 % so we can catch zero crossings. 0404 opts = odeset('reltol',1.e-9,'events',@ode_events); 0405 0406 ti = t; 0407 for i = 1:nt 0408 % si is the piece of arc length that we will look 0409 % for in this spline segment. 0410 si = s(i) - cumseglen(tbins(i)); 0411 0412 % extract polynomials for the derivatives 0413 % in the interval the point lies in 0414 for j = 1:ndim 0415 polyarray(j,:) = spld{j}.coefs(tbins(i),:); 0416 end 0417 0418 % we need to integrate in t, until the integral 0419 % crosses the specified value of si. Because we 0420 % have defined totalsplinelength, the lengths will 0421 % be normalized at this point to a unit length. 0422 % 0423 % Start the ode solver at -si, so we will just 0424 % look for an event where y crosses zero. 0425 [tout,yout,te,ye] = ode45(@(t,y) segkernel(t,y),[0,chordlen(tbins(i))],-si,opts); %#ok 0426 0427 % we only need that point where a zero crossing occurred 0428 % if no crossing was found, then we can look at each end. 0429 if ~isempty(te) 0430 ti(i) = te(1) + cumarc(tbins(i)); 0431 else 0432 % a crossing must have happened at the very 0433 % beginning or the end, and the ode solver 0434 % missed it, not trapping that event. 0435 if abs(yout(1)) < abs(yout(end)) 0436 % the event must have been at the start. 0437 ti(i) = tout(1) + cumarc(tbins(i)); 0438 else 0439 % the event must have been at the end. 0440 ti(i) = tout(end) + cumarc(tbins(i)); 0441 end 0442 end 0443 end 0444 0445 % Interpolate the parametric splines at ti to get 0446 % our interpolated value. 0447 for L = 1:ndim 0448 pt(:,L) = ppval(spl{L},ti); 0449 end 0450 0451 % do we need to compute first derivatives here at each point? 0452 if nargout > 1 0453 dudt = zeros(nt,ndim); 0454 for L = 1:ndim 0455 dudt(:,L) = ppval(spld{L},ti); 0456 end 0457 end 0458 0459 % create a function handle for evaluation, passing in the splines 0460 if nargout > 2 0461 fofthandle = @(t) foft(t,spl); 0462 end 0463 0464 % =============================================== 0465 % nested function for the integration kernel 0466 % =============================================== 0467 function val = segkernel(t,y) %#ok 0468 % sqrt((dx/dt)^2 + (dy/dt)^2 + ...) 0469 val = zeros(size(t)); 0470 for k = 1:ndim 0471 val = val + polyval(polyarray(k,:),t).^2; 0472 end 0473 val = sqrt(val); 0474 0475 end % function segkernel 0476 0477 % =============================================== 0478 % nested function for ode45 integration events 0479 % =============================================== 0480 function [value,isterminal,direction] = ode_events(t,y) %#ok 0481 % ode event trap, looking for zero crossings of y. 0482 value = y; 0483 isterminal = ones(size(y)); 0484 direction = ones(size(y)); 0485 end % function ode_events 0486 0487 end % mainline - interparc 0488 0489 0490 % =============================================== 0491 % end mainline - interparc 0492 % =============================================== 0493 % begin subfunctions 0494 % =============================================== 0495 0496 % =============================================== 0497 % subfunction for evaluation at any point externally 0498 % =============================================== 0499 function f_t = foft(t,spl) 0500 % tool allowing the user to evaluate the interpolant at any given point for any values t in [0,1] 0501 pdim = numel(spl); 0502 f_t = zeros(numel(t),pdim); 0503 0504 % convert t to a column vector, clipping it to [0,1] as we do. 0505 t = max(0,min(1,t(:))); 0506 0507 % just loop over the splines in the cell array of splines 0508 for i = 1:pdim 0509 f_t(:,i) = ppval(spl{i},t); 0510 end 0511 end % function foft 0512 0513 0514 function [str,errorclass] = validstring(arg,valid) 0515 % validstring: compares a string against a set of valid options 0516 % usage: [str,errorclass] = validstring(arg,valid) 0517 % 0518 % If a direct hit, or any unambiguous shortening is found, that 0519 % string is returned. Capitalization is ignored. 0520 % 0521 % arguments: (input) 0522 % arg - character string, to be tested against a list 0523 % of valid choices. Capitalization is ignored. 0524 % 0525 % valid - cellstring array of alternative choices 0526 % 0527 % Arguments: (output) 0528 % str - string - resulting choice resolved from the 0529 % list of valid arguments. If no unambiguous 0530 % choice can be resolved, then str will be empty. 0531 % 0532 % errorclass - string - A string argument that explains 0533 % the error. It will be one of the following 0534 % possibilities: 0535 % 0536 % '' --> No error. An unambiguous match for arg 0537 % was found among the choices. 0538 % 0539 % 'No match found' --> No match was found among 0540 % the choices provided in valid. 0541 % 0542 % 'Ambiguous argument' --> At least two ambiguous 0543 % matches were found among those provided 0544 % in valid. 0545 % 0546 % 0547 % Example: 0548 % valid = {'off' 'on' 'The sky is falling'} 0549 % 0550 % 0551 % See also: parse_pv_pairs, strmatch, strcmpi 0552 % 0553 % Author: John D'Errico 0554 % e-mail: woodchips@rochester.rr.com 0555 % Release: 1.0 0556 % Release date: 3/25/2010 0557 0558 ind = find(strncmpi(lower(arg),valid,numel(arg))); 0559 if isempty(ind) 0560 % No hit found 0561 errorclass = 'No match found'; 0562 str = ''; 0563 elseif (length(ind) > 1) 0564 % Ambiguous arg, hitting more than one of the valid options 0565 errorclass = 'Ambiguous argument'; 0566 str = ''; 0567 return 0568 else 0569 errorclass = ''; 0570 str = valid{ind}; 0571 end 0572 0573 end % function validstring 0574