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