1## Copyright (C) 2021 David Legland
2## All rights reserved.
3##
4## Redistribution and use in source and binary forms, with or without
5## modification, are permitted provided that the following conditions are met:
6##
7##     1 Redistributions of source code must retain the above copyright notice,
8##       this list of conditions and the following disclaimer.
9##     2 Redistributions in binary form must reproduce the above copyright
10##       notice, this list of conditions and the following disclaimer in the
11##       documentation and/or other materials provided with the distribution.
12##
13## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ''AS IS''
14## AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
15## IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
16## ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
17## ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
18## DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19## SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
20## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
21## OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
22## OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23##
24## The views and conclusions contained in the software and documentation are
25## those of the authors and should not be interpreted as representing official
26## policies, either expressed or implied, of the copyright holders.
27
28function varargout = polynomialCurveFit(t, varargin)
29%POLYNOMIALCURVEFIT Fit a polynomial curve to a series of points.
30%
31%   [XC YC] = polynomialCurveFit(T, XT, YT, ORDER)
32%   T is a Nx1 vector
33%   XT and YT are coordinate for each parameter value (column vectors)
34%   ORDER is the degree of the polynomial used for interpolation
35%   XC and YC are polynomial coefficients, given in ORDER+1 row vectors,
36%   starting from degree 0 and up to degree ORDER.
37%
38%	[XC YC] = polynomialCurveFit(T, POINTS, ORDER);
39%   specifies coordinate of points in a Nx2 array.
40%
41%   Example:
42%   N = 50;
43%   t = linspace(0, 3*pi/4, N)';
44%   xp = cos(t); yp = sin(t);
45%   [xc yc] = polynomialCurveFit(t, xp, yp, 3);
46%   curve = polynomialCurvePoint(t, xc, yc);
47%   drawCurve(curve);
48%
49%
50%	[XC YC] = polynomialCurveFit(..., T_I, COND_I);
51%   Impose some specific conditions. T_I is a value of the parametrization
52%   variable. COND_I is a cell array, with 2 columns, and as many rows as
53%   the derivatives specified for the given T_I. Format for COND_I is:
54%   COND_I = {X_I, Y_I; X_I', Y_I'; X_I", Y_I"; ...};
55%   with X_I and Y_I being the imposed coordinate at position T_I, X_I' and
56%   Y_I' being the imposed first derivatives, X_I" and Y_I" the imposed
57%   second derivatives, and so on...
58%   To specify a derivative without specifying derivative with lower
59%   degree, value of lower derivative can be let empty, using '[]'
60%
61%   Example:
62%   % defines a curve (circle arc) with small perturbations
63%   N = 100;
64%   t = linspace(0, 3*pi/4, N)';
65%   xp = cos(t)+.1*randn(size(t)); yp = sin(t)+.1*randn(size(t));
66%
67%   % plot the points
68%   figure(1); clf; hold on;
69%   axis([-1.2 1.2 -.2 1.2]); axis equal;
70%   drawPoint(xp, yp);
71%
72%   % fit without knowledge on bounds
73%   [xc0 yc0] = polynomialCurveFit(t, xp, yp, 5);
74%   curve0 = polynomialCurvePoint(t, xc0, yc0);
75%   drawCurve(curve0);
76%
77%   % fit by imposing coordinate on first point
78%   [xc1 yc1] = polynomialCurveFit(t, xp, yp, 5, 0, {1, 0});
79%   curve1 = polynomialCurvePoint(t, xc1, yc1);
80%   drawCurve(curve1, 'r');
81%
82%   % fit by imposing coordinate (1,0) and derivative (0,1) on first point
83%   [xc2 yc2] = polynomialCurveFit(t, xp, yp, 5, 0, {1, 0;0 1});
84%   curve2 = polynomialCurvePoint(t, xc2, yc2);
85%   drawCurve(curve2, 'g');
86%
87%   % fit by imposing several conditions on various points
88%   [xc3 yc3] = polynomialCurveFit(t, xp, yp, 5, ...
89%       0, {1, 0;0 1}, ...      % coord and first derivative of first point
90%       3*pi/4, {-sqrt(2)/2, sqrt(2)/2}, ...    % coord of last point
91%       pi/2, {[], [];-1, 0});      % derivative of point on the top of arc
92%   curve3 = polynomialCurvePoint(t, xc3, yc3);
93%   drawCurve(curve3, 'k');
94%
95%   Requires the optimization Toolbox.
96%
97%
98%   Examples:
99%   polynomialCurveFit
100%
101%   See also
102%   polynomialCurves2d
103%
104% ------
105% Author: David Legland
106% e-mail: david.legland@grignon.inra.fr
107% Created: 2007-02-27
108% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.
109
110
111%% extract input arguments
112
113% extract curve coordinate
114var = varargin{1};
115if min(size(var))==1
116    % curve given as separate arguments
117    xt = varargin{1};
118    yt = varargin{2};
119    varargin(1:2)=[];
120else
121    % curve coordinate bundled in a matrix
122    if size(var, 1)<size(var, 2)
123        var = var';
124    end
125    xt = var(:,1);
126    yt = var(:,2);
127    varargin(1)=[];
128end
129
130% order of the polynom
131var = varargin{1};
132if length(var)>1
133    Dx = var(1);
134    Dy = var(2);
135else
136    Dx = var;
137    Dy = var;
138end
139varargin(1)=[];
140
141
142%% Initialize local conditions
143
144% For a solution vector 'x', the following relation must hold:
145%   Aeq * x == beq,
146% with:
147%   Aeq   Matrix M*N
148%   beq   column vector with length M
149% The coefficients of the Aeq matrix are initialized as follow:
150% First point and last point are considered successively. For each point,
151% k-th condition is the value of the (k-1)th derivative. This value is
152% computed using relation of the form:
153%   value = sum_i ( fact(i) * t_j^pow(i) )
154% with:
155%   i     indice of the (i-1) derivative.
156%   fact  row vector containing coefficient of each power of t, initialized
157%       with a row vector equals to [1 1 ... 1], and updated for each
158%       derivative by multiplying by corresponding power minus 1.
159%   pow   row vector of the powers of each monome. It is represented by a
160%       row vector containing an increasing series of power, eventually
161%       completed with zeros for lower degrees (for the k-th derivative,
162%       the coefficients with power lower than k are not relevant).
163
164% Example for degree 5 polynom:
165%   iter deriv  pow                 fact
166%   1    0      [0 1 2 3 4 5]       [1 1 1 1 1 1]
167%   2    1      [0 0 1 2 3 4]       [0 1 2 3 4 5]
168%   3    2      [0 0 0 1 2 3]       [0 0 1 2 3 4]
169%   4    3      [0 0 0 0 1 2]       [0 0 0 1 2 3]
170%   ...
171%   The process is repeated for coordinate x and for coordinate y.
172
173% Initialize empty matrices
174Aeqx = zeros(0, Dx+1);
175beqx = zeros(0, 1);
176Aeqy = zeros(0, Dy+1);
177beqy = zeros(0, 1);
178
179% Process local conditions
180while ~isempty(varargin)
181    if length(varargin)==1
182        warning('MatGeom:PolynomialCurveFit:ArgumentNumber', ...
183            'Wrong number of arguments in polynomialCurvefit');
184    end
185
186    % extract parameter t, and cell array of local conditions
187    ti = varargin{1};
188    cond = varargin{2};
189
190    % factors for coefficients of each polynomial. At the beginning, they
191    % all equal 1. With successive derivatives, their value increase by the
192    % corresponding powers.
193    factX = ones(1, Dx+1);
194    factY = ones(1, Dy+1);
195
196    % start condition initialisations
197    for i = 1:size(cond, 1)
198        % degrees of each polynomial
199        powX = [zeros(1, i) 1:Dx+1-i];
200        powY = [zeros(1, i) 1:Dy+1-i];
201
202        % update conditions for x coordinate
203        if ~isempty(cond{i,1})
204            Aeqx = [Aeqx ; factY.*power(ti, powX)]; %#ok<AGROW>
205            beqx = [beqx; cond{i,1}]; %#ok<AGROW>
206        end
207
208        % update conditions for y coordinate
209        if ~isempty(cond{i,2})
210            Aeqy = [Aeqy ; factY.*power(ti, powY)]; %#ok<AGROW>
211            beqy = [beqy; cond{i,2}]; %#ok<AGROW>
212        end
213
214        % update polynomial degrees for next derivative
215        factX = factX.*powX;
216        factY = factY.*powY;
217    end
218
219    varargin(1:2)=[];
220end
221
222
223%% Initialisations
224
225% ensure column vectors
226t  = t(:);
227xt = xt(:);
228yt = yt(:);
229
230% number of points to fit
231L = length(t);
232
233
234%% Compute coefficients of each polynomial
235
236% avoid optimization warnings
237warning('off', 'optim:lsqlin:LinConstraints');
238
239% options to turn display off
240options = optimset('display', 'off');
241
242% main matrix for x coordinate, size L*(degX+1)
243T = ones(L, Dx+1);
244for i = 1:Dx
245    T(:, i+1) = power(t, i);
246end
247
248% compute interpolation
249xc = lsqlin(T, xt, zeros(1, Dx+1), 1, Aeqx, beqx, [], [], [], options)';
250
251% main matrix for y coordinate, size L*(degY+1)
252T = ones(L, Dy+1);
253for i = 1:Dy
254    T(:, i+1) = power(t, i);
255end
256
257% compute interpolation
258yc = lsqlin(T, yt, zeros(1, Dy+1), 1, Aeqy, beqy, [], [], [], options)';
259
260
261%% Format output arguments
262if nargout <= 1
263    varargout{1} = {xc, yc};
264else
265    varargout{1} = xc;
266    varargout{2} = yc;
267end
268