%
% MATLAB for DUMMIES 11-12
% Exercice 4
%
% Solution detaillee
%  Vincent Legat, Damien Bouvy, Denis-Gabriel Caprace, Gilles De Neyer, 
%  Kevin Degraux, Mohammad Iranmanesh, Christophe Lorant, Remy Moulin, 
%  Matthew Philippe, Karim Slaoui, Olivier Thiry, Laurent Van Eesbeeck,
%  Thomas Vanderstraeten, Antoine Weynants
%
% -------------------------------------------------------------------------
%
function matlab4() 

p = 2;
T =  [0 0 0 1 2 3 4 5 6 7 8 8 8];
X = [1  1  0 -6 -2 -1  7  1  1   -4]; 
Y = [0  1  4  1  4 -2 -2 -1  8 -6];
W = [1 1 1 1 3 1 2 1 1 4];
L = computeLength(T,X,Y,W,p);
Lref = 41.00979861;
fprintf('  === Result : %12.8f (%12.8f) \n',L,Lref);

p = 3;
T = [0 0 0 1 2 3 4 5 6 7 9 9 9 9];
X = [1  1  0 -6 -2 -1  7  1  1   -4]; 
Y = [0  1  4  1  4 -2 -2 -1  8 -6];
W = [1 1 1 1 3 1 2 1 1 4];
L = computeLength(T,X,Y,W,p);
Lref = 32.66481105;    
fprintf('  === Result : %12.8f (%12.8f) \n',L,Lref);
 
p = 3;
T =  [0 0 0 0 2 3 4 5 6 7 9 9 9 9];
X = [1  1  0 -6 -2 -1  7  1  1   -4]; 
Y = [0  1  4  1  4 -2 -2 -1  8 -6];
W = [1 1 1 1 3 1 2 1 1 4];
L = computeLength(T,X,Y,W,p);
Lref = 33.97960235;
fprintf('  === Result : %12.8f (%12.8f) \n',L,Lref);

nt = length(T) - 1;
dt = 0.005;
t = [T(p+1):dt:T(nt-p+1)];
for i=0:nt-p-1
  B(i+1,:) = b(t,T,i,p);
end

w = W * B;
x =(W .* X) * B ./ w;
y =(W .* Y) * B ./ w;

plot(x,y,'-r',X,Y,'.b','MarkerSize',25);
axis equal; axis off

end


function L = computeLength(T,X,Y,W,p)
%COMPUTELENGTH Computes the length of a NURBS 2D curve
%   [L] = COMPUTELENGTH(T,X,Y,W,P) computes the length of a NURBS 
%   curve of order p defined by nodes T, control points (X,Y) and
%   weights W. 
%   The integration quadrature is a 2 points Gauss-Legendre rule.


%
% -1- Computation of all integration points and nodes
%     Empty intervals do not need to be detected as their respective
%     weight is simply zero (yes :-)
%
%     Usual mistakes :
%       - You have to integrate ONLY between T_(p+1) and T_(nt-p+1)
%       - Do not forget the jacobian factor !
%       - In an unfair way, the provided test script fails to detect both
%         usual errors (it was unexpected, in fact)
%

nt  = length(T) - 1;
h   = T(p+2:nt-p+1) - T(p+1:nt-p);
t   = (T(p+2:nt-p+1) + T(p+1:nt-p))/2;
t   = [ t-h/(2*sqrt(3)) t+h/(2*sqrt(3)) ];
jac = [h/2 h/2];

%
% -2- Numerical integration
%     Observe the preallocation of B and dBdt :-)
%     All operations are vectorized
%

B  = zeros(nt-p,numel(t)); dBdt = B;
for i=0:nt-p-1
  B(i+1,:) = b(t,T,i,p);
  dBdt(i+1,:) = dbdt(t,T,i,p);
end

w = W * B;
x =(W .* X) * B ./ w;
y =(W .* Y) * B ./ w;
dwdt = W * dBdt;
dxdt = (W .* X) * dBdt ./ w - x .* dwdt ./ w;
dydt = (W .* Y) * dBdt ./ w - y .* dwdt ./ w;
L = sum (sqrt(dxdt.*dxdt + dydt .* dydt).*jac);

end

% 
% ===== B-Splines mini-library =====================================
%

function u = b(t,T,j,p)
%
% A simple (but not efficient) way of computing B-splines functions
%
i = j+1;
if p==0
    u = (t>= T(i) & t < T(i+p+1)); return 
end

u = zeros(size(t));
if T(i) ~= T(i+p)
    u = u + ((t-T(i)) ./ (T(i+p) -T(i))) .* b(t,T,j,p-1);
end
if T(i+1) ~= T(i+p+1)
    u = u + ((T(i+p+1)-t) ./ (T(i+p+1) -T(i+1))) .* b(t,T,j+1,p-1);
end
end

function u = dbdt(t,T,j,p)
%
% Derivatives of the B-splines functions
%
i = j+1;
if p==0
    u = (t>= T(i) & t < T(i+p+1)); return 
end

u = zeros(size(t));
if T(i) ~= T(i+p)
    u = u + (p / (T(i+p) -T(i))) * b(t,T,j,p-1);
end
if T(i+1) ~= T(i+p+1)
    u = u - (p / (T(i+p+1) -T(i+1))) * b(t,T,j+1,p-1);
end
end