--- /dev/null
+%% Copyright (C) 2003-2011 David Legland <david.legland@grignon.inra.fr>
+%% Copyright (C) 2012 Adapted to Octave by Juan Pablo Carbajal <carbajal@ifi.uzh.ch>
+%% All rights reserved.
+%%
+%% Redistribution and use in source and binary forms, with or without
+%% modification, are permitted provided that the following conditions are met:
+%%
+%% 1 Redistributions of source code must retain the above copyright notice,
+%% this list of conditions and the following disclaimer.
+%% 2 Redistributions in binary form must reproduce the above copyright
+%% notice, this list of conditions and the following disclaimer in the
+%% documentation and/or other materials provided with the distribution.
+%%
+%% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ''AS IS''
+%% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+%% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+%% ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
+%% ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+%% DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+%% SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+%% CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+%% OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+%% OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+%%
+%% The views and conclusions contained in the software and documentation are
+%% those of the authors and should not be interpreted as representing official
+%% policies, either expressed or implied, of the copyright holders.
+
+%% -*- texinfo -*-
+%% @deftypefn {Function File} {@var{kappa} = } curvature (@var{t}, @var{px}, @var{py},@var{method},@var{degree})
+%% @deftypefnx {Function File} {@var{kappa} = } curvature (@var{t}, @var{poly},@var{method},@var{degree})
+%% @deftypefnx {Function File} {@var{kappa} = } curvature (@var{px}, @var{py},@var{method},@var{degree})
+%% @deftypefnx {Function File} {@var{kappa} = } curvature (@var{points},@var{method},@var{degree})
+%% @deftypefnx {Function File} {[@var{kappa} @var{poly} @var{t}] = } curvature (@dots{})
+%% Estimate curvature of a polyline defined by points.
+%%
+%% First compute an approximation of the curve given by PX and PY, with
+%% the parametrization @var{t}. Then compute the curvature of approximated curve
+%% for each point.
+%% @var{method} used for approximation can be only: 'polynom', with specified degree.
+%% Further methods will be provided in a future version.
+%% @var{t}, @var{px}, and @var{py} are N-by-1 array of the same length. The points
+%% can be specified as a single N-by-2 array.
+%%
+%% If the argument @var{t} is not given, the parametrization is estimated using
+%% function @code{parametrize}.
+%%
+%% If requested, @var{poly} contains the approximating polygon evlauted at the
+%% parametrization @var{t}.
+%%
+%% @seealso{parametrize, polygons2d}
+%% @end deftypefn
+
+function [kappa, varargout] = curvature(varargin)
+
+ % default values
+ degree = 5;
+ t=0; % parametrization of curve
+ tc=0; % indices of points wished for curvature
+
+
+ % =================================================================
+
+ % Extract method and degree ------------------------------
+
+ nargin = length(varargin);
+ varN = varargin{nargin};
+ varN2 = varargin{nargin-1};
+
+ if ischar(varN2)
+ % method and degree are specified
+ method = varN2;
+ degree = varN;
+ varargin = varargin(1:nargin-2);
+ elseif ischar(varN)
+ % only method is specified, use degree 6 as default
+ method = varN;
+ varargin = varargin{1:nargin-1};
+ else
+ % method and degree are implicit : use 'polynom' and 6
+ method = 'polynom';
+ end
+
+ % extract input parametrization and curve. -----------------------
+ nargin = length(varargin);
+ if nargin==1
+ % parameters are just the points -> compute caracterization.
+ var = varargin{1};
+ px = var(:,1);
+ py = var(:,2);
+ elseif nargin==2
+ var = varargin{2};
+ if size(var, 2)==2
+ % parameters are t and POINTS
+ px = var(:,1);
+ py = var(:,2);
+ t = varargin{1};
+ else
+ % parameters are px and py
+ px = varargin{1};
+ py = var;
+ end
+ elseif nargin==3
+ var = varargin{2};
+ if size(var, 2)==2
+ % parameters are t, POINTS, and tc
+ px = var(:,1);
+ py = var(:,2);
+ t = varargin{1};
+ else
+ % parameters are t, px and py
+ t = varargin{1};
+ px = var;
+ py = varargin{3};
+ end
+ elseif nargin==4
+ % parameters are t, px, py and tc
+ t = varargin{1};
+ px = varargin{2};
+ py = varargin{3};
+ tc = varargin{4};
+ end
+
+ % compute implicit parameters --------------------------
+
+ % if t and/or tc are not computed, use implicit definition
+ if t==0
+ t = parametrize(px, py, 'norm');
+ end
+
+ % if tc not defined, compute curvature for all points
+ if tc==0
+ tc = t;
+ else
+ % else convert from indices to parametrization values
+ tc = t(tc);
+ end
+
+
+ % =================================================================
+ % compute curvature for each point of the curve
+
+ if strcmp(method, 'polynom')
+ % compute coefficients of interpolation functions
+ x0 = polyfit(t, px, degree);
+ y0 = polyfit(t, py, degree);
+
+ % compute coefficients of first and second derivatives. In the case of a
+ % polynom, it is possible to compute coefficient of derivative by
+ % multiplying with a matrix.
+ derive = diag(degree:-1:0);
+ xp = circshift(x0*derive, [0 1]);
+ yp = circshift(y0*derive, [0 1]);
+ xs = circshift(xp*derive, [0 1]);
+ ys = circshift(yp*derive, [0 1]);
+
+ % compute values of first and second derivatives for needed points
+ xprime = polyval(xp, tc);
+ yprime = polyval(yp, tc);
+ xsec = polyval(xs, tc);
+ ysec = polyval(ys, tc);
+
+ % compute value of curvature
+ kappa = (xprime.*ysec - xsec.*yprime)./ ...
+ power(xprime.*xprime + yprime.*yprime, 3/2);
+
+ if nargout > 1
+ varargout{1} = [polyval(x0,tc(:)) polyval(y0,tc(:))];
+ if nargout > 2
+ varargout{2} = tc;
+ end
+ end
+ else
+ error('unknown method');
+ end
+
+endfunction