function varargout = chol( f, varargin )
%CHOL    Cholesky factorization of a SEPARABLEAPPROX.
%
% R = CHOL( F ), if F is a nonnegative definite SEPARABLEAPPROX then this
% returns an upper triangular quasimatrix so that R'*R is a
% decomposition of F. If F is not nonnegative definite then an error is thrown.
%
% L = CHOL(F, 'lower'), if F is a nonnegative definite SEPARABLEAPPROX then this
% produces a lower triangular quasimatrix so that L*L' is a decomposition of F.
% If F is not nonnegative definite then an error is thrown.
%
% [R, p] = CHOL( F ), with two outputs never throwns an error message. If F is
% nonnegative definite then p is 0 and R is the same as above. If F is
% symmetric but negative definite or semidefinite then p is a positive
% integer such that R has p columns and R'*R is a rank p nonnegative definite
% SEPARABLEAPPROX that approximates F.
% This is particular useful when F is nonnegative definite, but rounding errors
% have perturbed it to be semidefinite.
%
% [L, p] = CHOL(F, 'lower') same as above but the first argument is lower
% triangular.
%
% For more information about the factorization:
% A. Townsend and L. N. Trefethen, Continuous analogues of matrix
% factorizations, Proc. Royal Soc. A., 2015.
%
% See also LU, and QR.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check:
if ( isempty( f ) )
    varargout = cell(1, nargout);
    return
end

% Is the separableApprox on a square domain?:
if ( ~domainCheck(f.cols, f.rows) )
    error('CHEBFUN:SEPARABLEAPPROX:chol:domain', ...
        'SEPARABLEAPPROX is not on a square domain.');
end

% Get rank of f:
k = length( f );

% All the pivots should be on the y = x line.
PivLoc = f.pivotLocations;

if ( isempty( PivLoc ) )
    % For some reason (probably because f was made with sampling data, rather
    % than a handle) f didn't have any pivot information. Make it now:
    f = compose(f, @plus, 0 );
    varargout = {chol( f )};
    return
end

% Find the first pivot location is off-diagonal:
Diagk = find( PivLoc(:,1) ~= PivLoc(:,2), 1, 'first');
k = min( k, Diagk );

% Check that the pivots are positive:
Posk = find( pivots( f ) < 0, 1, 'first');
k = min(k, Posk);

% Were all the pivots nonnegative and the locations on the diagonal?
posdef = ( k == length( f ) );

% Return an error if the function is not nonnegative definite:
if ( nargout < 2 && ~isempty( posdef ) && ~posdef )
    error('CHEBFUN:SEPARABLEAPPROX:chol:definite', ...
        'SEPARABLEAPPROX is not nonnegative definite.');
end

% Get the CDR decomposition (already computed by constructor):
[C, D, R] = cdr( f );

% Return an error if the function is not symmetric:
dom = f.domain;
r = 0.0192475;   % arbitrary point in [-1,1]
s = -.34756987;  % arbitrary point in [-1,1]
r = diff(dom(1:2))/2*r + mean(dom(1:2));
s = diff(dom(3:4))/2*s + mean(dom(3:4));
symTest = abs(feval(f,r,s) - feval(f,s,r)) < 1e2*eps;
if ( ~symTest )
    error('CHEBFUN:SEPARABLEAPPROX:chol:symmetric', ...
        'The SEPARABLEAPPROX must be a symmetric function.');
end

% How many terms are posdef:
p = k;
if ( isempty( p ) )
    p = length( f );
end
% Extract out posdef part:
C = C( :, 1:p );
D = D( 1:p, 1:p );
R = R( :, 1:p );

% Balance scaling:
C = C * sqrt( D );
R = R * sqrt( D );

% Output to user:
if ( ( nargout < 2 ) && ( nargin == 1 ) )
    varargout = { R.' };
elseif ( ( nargout < 2 ) && strcmpi( varargin{ 1 }, 'lower') )
    varargout = { C };
elseif ( ( nargout == 2 ) && ( nargin == 1 ) )
    varargout = { R.', p };
elseif ( ( nargout == 2 ) && strcmpi( varargin{ 1 }, 'lower') )
    varargout = { C, p };
end

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function [y,m] = getPathsAsian(S0, r, sigma, dt, NSim, Nr)
% path retrieval for Asian options

if length(S0) > 1
    lenS = length(S0);             % for using this with different starting values
    R = exp((r - sigma^2/2) * dt ...
    + sigma * sqrt(dt) * randn(NSim,Nr,lenS));   % random noise + drift
    tmp = zeros(NSim,1,lenS);
    tmp(:,1,:) = repmat(log(S0)',NSim,1);
    y = exp(cumsum([tmp, R], 2));    % path set simple example
else
    R = exp((r - sigma^2/2) * dt ...
    + sigma * sqrt(dt) * randn(NSim,Nr));
    y = cumprod([S0*ones(NSim,1), R], 2); %S0 in Spalte 1 -> Indexverschiebung

end

tmp = repmat(1:Nr, NSim, 1); %Hilfsmatrix
m = [y(:,1) cumsum(y(:,2:end), 2) ./ tmp];

end


classdef CEC2017_F3 < PROBLEM
% <single> <real> <constrained>
% CEC'2017 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% G. Wu, R. Mallipeddi, and P. N. Suganthan, Problem definitions and
% evaluation criteria for the CEC 2017 competition on constrained real-
% parameter optimization, National University of Defense Technology, China,
% 2016.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2017.mat'),'Data');
            obj.O = Data{3};
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = sum(cumsum(Z,2).^2,2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopCon(:,1) = sum(Z.^2-5000*cos(0.1*pi*Z)-4000,2);
            PopCon(:,2) = abs(sum(Z.*sin(0.1*pi*Z),2)) - 1e-4;
        end
    end
end


function [u,p,edge,A,eqn,info] = StokesP2P1(node,elem,pde,bdFlag,option)
%% STOKESP2P1 Stokes equation: P2-P1 Taylor-Hood elements.
%
%   [u,p] = STOKESP2P1(node,elem,pde,bdFlag) use quadratic and piceswise
%   linear elements to approximate velocity u and pressure p, repectively.
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%   with
%       Dirichlet boundary condition        u = g_D  on \Gamma_D,
%       Neumann boundary condition du/dn - np = g_N  on \Gamma_N.
%
%   It is a choice of option.fem in Stokes. Please read Stokes for more
%   information on the input and output.
%
% See also Stokes, Poisson, StokesP2P1
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end

tic;
%% Construct Data Structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1);  Nu = N+size(edge,1);   Np = N;

%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix for Laplace operator
% generate sparse pattern
ii = zeros(21*NT,1); jj = zeros(21*NT,1);
index = 0;
for i = 1:6
    for j = i:6
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        index = index + NT;
    end
end
% quadrature points
if ~isfield(pde,'nu'), pde.nu = []; end
if ~isfield(option,'quadorder')
    % constant viscosity
    option.quadorder = 2;        % default order
    if ~isempty(pde.nu) && isnumeric(pde.nu) % numerical viscosity
        option.quadorder = 3;    % exact for linear diffusion coefficient
    end
end
[lambda, w] = quadpts(option.quadorder);
nQuad = size(lambda,1);
% compute non-zeros
sA = zeros(21*NT,nQuad);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    index = 0;
    for i = 1:6
        for j = i:6
            Aij = 0;
            if isempty(pde.nu) || isnumeric(pde.nu)
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
            else
                pxy = lambda(p,1)*node(elem(:,1),:) ...
                    + lambda(p,2)*node(elem(:,2),:) ...
                    + lambda(p,3)*node(elem(:,3),:);
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
            end
            if ~isempty(pde.nu) && (pde.nu~=1)
                Aij = pde.nu*Aij;
            end
            Aij = Aij.*area;
            sA(index+1:index+NT,p) = Aij;
            index = index + NT;
        end
    end
end
sA = sum(sA,2);
% assemble the matrix
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Nu,Nu);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Nu,Nu);
A = A + AU + AU';
A = blkdiag(A,A);
clear Aij ii jj sA

%% Assemble the matrix for divergence operator
Dx = sparse(Np,Nu);
Dy = sparse(Np,Nu);
[lambda, w] = quadpts(2); % (div(P2), P1) is P2
nQuad = size(lambda,1);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    for i = 1:6
        for j = 1:3
            Dxij = 0;
            Dyij = 0;
            Dxij = Dxij + w(p)*Dphip(:,1,i).*lambda(p,j);
            Dyij = Dyij + w(p)*Dphip(:,2,i).*lambda(p,j);
            Dx = Dx + sparse(elem(:,j),double(elem2dof(:,i)),Dxij.*area,Np,Nu);
            Dy = Dy + sparse(elem(:,j),double(elem2dof(:,i)),Dyij.*area,Np,Nu);
        end
    end
end
B = -[Dx Dy];
clear Dxij Dyij Dx Dy Dphip

%% Assemble right hand side
f1 = zeros(Nu,1);
f2 = zeros(Nu,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,weight] = quadpts(option.fquadorder);
    % basis values at quadrature points
    phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
    phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
    phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
    phi(:,4) = 4*lambda(:,2).*lambda(:,3);
    phi(:,5) = 4*lambda(:,3).*lambda(:,1);
    phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    nQuad = size(lambda,1);
    ft1 = zeros(NT,6);
    ft2 = zeros(NT,6);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        % function values at quadrature points
        fp = pde.f(pxy);
        % evaluate fp outside.
        for j = 1:6
            ft1(:,j) = ft1(:,j) + fp(:,1).*phi(p,j)*weight(p);
            ft2(:,j) = ft2(:,j) + fp(:,2).*phi(p,j)*weight(p);
        end
    end
    ft1 = ft1.*repmat(area,1,6);
    ft2 = ft2.*repmat(area,1,6);
    f1 = accumarray(elem2dof(:),ft1(:),[Nu 1]);
    f2 = accumarray(elem2dof(:),ft2(:),[Nu 1]);
end
clear phi ft1 ft2

%% Boundary Conditions
[AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesP2P1;

%% Record assembeling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(ufreeDof), return; end
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if length(f)+length(g) <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else          % Multigrid-type  solver for large size systems
        option.solver = 'asmg';
    end
end
solver = option.solver;

% solve
switch solver
    case 'direct'
        tic
        bigA = [AD, B'; ...
                B, sparse(Np,Np)];
        bigF = [f; g];
        bigu = [u; p];
        bigFreeDof = [ufreeDof; 2*Nu+pDof];
        bigu(bigFreeDof) = bigA(bigFreeDof,bigFreeDof)\bigF(bigFreeDof);
        u = bigu(1:2*Nu);
        p = bigu(2*Nu+1:end);
        residual = norm(bigF - bigA*bigu);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'mg'
%         option.tol = Np^(-2);
        option.solver  = 'WCYCLE';
        [u(ufreeDof),p,info] = mgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                        u(ufreeDof),p,elem,ufreeDof,option);
    case 'asmg'
        [u(ufreeDof),p,info] = asmgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                          u,p,node,elem,bdFlag,ufreeDof,option);
end

%% Post-process
if length(pDof) ~= Np % p is unique up to a constant
    % impose the condition int(p)=0
    c = sum(mean(p(elem),2).*area)/sum(area);
    p = p - c;
end

%% Output information
eqn = struct('A',AD,'B',BD,'f',f,'g',g,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdStokesP2P1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesP2P1
    %% Boundary condition of Stokes equation: P2-P0 elements

    %% Initial set up
%     f = [f1; f2];    % set in Neumann boundary condition
    g = zeros(Np,1);
    u = zeros(2*Nu,1);
    p = zeros(Np,1);
    ufreeDof = (1:Nu)';
    pDof = (1:Np)';

    if ~exist('bdFlag','var'), bdFlag = []; end
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Find Dirichlet dof and modify the matrix
    % Find Dirichlet boundary dof: fixedDof and pDof
    isFixedDof = false(Nu,1);
    if ~isempty(bdFlag)       % case: bdFlag is not empty
        elem2edge = elem2dof(:,4:6)-N;
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;   % nodes of all D-edges
        isFixedDof(N + find(isDirichlet')) = true;% dof on D-edges
        fixedDof = find(isFixedDof);
        ufreeDof = find(~isFixedDof);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        fixedDof = bdDof;
        isFixedDof(fixedDof) = true;
        ufreeDof = find(~isFixedDof);
    end
    if isempty(fixedDof) % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        fixedDof = 1;
        ufreeDof = (2:Nu)';    % eliminate the kernel by enforcing u(1) = 0;
    end

    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedDof,fixedDof)=I, AD(fixedDof,ufreeDof)=0, AD(ufreeDof,fixedDof)=0.
    % BD(:,fixedDof) = 0 and thus BD'(fixedDof,:) = 0.
    bdidx = zeros(2*Nu,1);
    bdidx([fixedDof; Nu+fixedDof]) = 1;
    Tbd = spdiags(bdidx,0,2*Nu,2*Nu);
    T = spdiags(1-bdidx,0,2*Nu,2*Nu);
    AD = T*A*T + Tbd;
    BD = B*T;

    %% Part 2: Find boundary edges and modify the right hand side f and g
    % Find boundary edges: Neumann and Robin
    Neumann = []; Robin = []; %#ok<*NASGU>
    if ~isempty(bdFlag)
        isNeumann(elem2edge((bdFlag(:)==2)|(bdFlag(:) == 3))) = true;
        isRobin(elem2edge(bdFlag(:)==3)) = true;
        Neumannidx = find(isNeumann);
        Neumann   = edge(isNeumann,:);
        Robin     = edge(isRobin,:);
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        Neumann = edge(bdDof>N,:);
        if ~isempty(pde.g_R)
            Robin = Neumann;
        end
    end

    % Neumann boundary condition
    if ~isempty(pde.g_N) && ~isempty(Neumann) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        [lambda,w] = quadpts1(3);
        nQuad = size(lambda,1);
        % quadratic bases (1---3---2)
        bdphi(:,1) = (2*lambda(:,1)-1).*lambda(:,1);
        bdphi(:,2) = (2*lambda(:,2)-1).*lambda(:,2);
        bdphi(:,3) = 4*lambda(:,1).*lambda(:,2);
        % length of edge
        ve = node(Neumann(:,1),:) - node(Neumann(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        % update RHS
        gex = zeros(size(Neumann,1),2);   % x-component
        gey = zeros(size(Neumann,1),2);   % y-component
        for pp = 1:nQuad
            pxy = lambda(pp,1)*node(Neumann(:,1),:)+lambda(pp,2)*node(Neumann(:,2),:);
            gp = pde.g_N(pxy);
            gex(:,1) = gex(:,1) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,1);
            gex(:,2) = gex(:,2) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,2);
            gey(:,1) = gey(:,1) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,1);
            gey(:,2) = gey(:,2) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,2);
            f1(N+Neumannidx) = f1(N+Neumannidx) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,3); % interior bubble
            f2(N+Neumannidx) = f2(N+Neumannidx) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,3); % interior bubble
        end
        f1(1:N) = f1(1:N) + accumarray(Neumann(:), gex(:),[N,1]);
        f2(1:N) = f2(1:N) + accumarray(Neumann(:), gey(:),[N,1]);
    end
    f = [f1; f2];
    % The case non-empty Neumann but g_N=[] corresponds to the zero flux
    % boundary condition on Neumann edges and no modification is needed.

    % Dirichlet boundary conditions
    if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
        u1 = zeros(Nu,1);
        u2 = zeros(Nu,1);
        idx = (fixedDof > N);              % index of edge dof
        uD = pde.g_D(node(fixedDof(~idx),:));  % bd value at vertex dofs
        u1(fixedDof(~idx)) = uD(:,1);
        u2(fixedDof(~idx)) = uD(:,2);
        bdEdgeIdx = fixedDof(idx)-N;
        bdEdgeMid = (node(edge(bdEdgeIdx,1),:)+node(edge(bdEdgeIdx,2),:))/2;
        uD = pde.g_D(bdEdgeMid);         % bd values at middle points of edges
        u1(fixedDof(idx)) = uD(:,1);
        u2(fixedDof(idx)) = uD(:,2);
        u = [u1; u2]; % Dirichlet bd condition is built into u
        f = f - A*u;  % bring affect of nonhomgenous Dirichlet bd condition to
        g = g - B*u;  % the right hand side
        g = g - mean(g);
        f(fixedDof) = u1(fixedDof);
        f(fixedDof+Nu) = u2(fixedDof);
    end
    % The case non-empty Dirichlet but g_D=[] corresponds to the zero Dirichlet
    % boundary condition and no modification is needed.

    % modfiy pressure dof for pure Dirichlet
    if isempty(Neumann)
        pDof = (1:Np-1)';
    end

    ufreeDof = [ufreeDof; Nu+ufreeDof];
    end % end of function getbdStokesP2P1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


%QUATERNION	constructor for quaternion objects
%
% 	QUATERNION([s v1 v2 v3])	from 4 elements
% 	QUATERNION(v, theta)		from vector plus angle
% 	QUATERNION(R)			from a 3x3 or 4x4 matrix
% 	QUATERNION(q)			from another quaternion

% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function q = Quaternion(a1, a2)

	if nargin == 0,
		q.s = 1;
		q.v = [0 0 0];
		q = class (q, 'Quaternion');

	elseif nargin == 1
		if isa(a1, 'Quaternion')
			q = a1;
			q = class(q, 'Quaternion');
		elseif isreal (a1) && size(a1) == 1
			q.s = a1(1);
			q.v = [0,0,0];
			q = class(q, 'Quaternion');
		elseif isreal (a1) && all (size (a1) == [1 3]) # Quaternion (vector part)
			q.s = 0;
			q.v = a1(1:3);
			q = class(q, 'Quaternion');
		elseif all(size(a1) == [3 3])
			q = Quaternion( tr2q(a1) );
		elseif all(size(a1) == [4 4])
			q = Quaternion( tr2q(a1(1:3,1:3)) );
		elseif all(size(a1) == [1 4])
			q.s = a1(1);
			q.v = a1(2:4);
			q = class(q, 'Quaternion');
		else
			error('unknown dimension of input');
		end
	elseif nargin == 2
		if  isscalar(a1) && isvector(a2)
			q.s = cos(a1/2);
			q.v = (sin(a1/2)*unit(a2(:)'));
			q = class(q, 'Quaternion');
		end
	end
endfunction
%TR2Q	Convert homogeneous transform to a unit-quaternion
%
%	Q = tr2q(T)
%
%	Return a unit quaternion corresponding to the rotational part of the
%	homogeneous transform T.
%
%	See also Q2TR

% Ryan Steidnl based on Robotics Toolbox for MATLAB (v6 and v9)
%

% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function q = tr2q(t)
	qs = sqrt(trace(t)+1)/2.0;
	kx = t(3,2) - t(2,3);	% Oz - Ay
	ky = t(1,3) - t(3,1);	% Ax - Nz
	kz = t(2,1) - t(1,2);	% Ny - Ox

	if (t(1,1) >= t(2,2)) & (t(1,1) >= t(3,3))
		kx1 = t(1,1) - t(2,2) - t(3,3) + 1;	% Nx - Oy - Az + 1
		ky1 = t(2,1) + t(1,2);			% Ny + Ox
		kz1 = t(3,1) + t(1,3);			% Nz + Ax
		add = (kx >= 0);
	elseif (t(2,2) >= t(3,3))
		kx1 = t(2,1) + t(1,2);			% Ny + Ox
		ky1 = t(2,2) - t(1,1) - t(3,3) + 1;	% Oy - Nx - Az + 1
		kz1 = t(3,2) + t(2,3);			% Oz + Ay
		add = (ky >= 0);
	else
		kx1 = t(3,1) + t(1,3);			% Nz + Ax
		ky1 = t(3,2) + t(2,3);			% Oz + Ay
		kz1 = t(3,3) - t(1,1) - t(2,2) + 1;	% Az - Nx - Oy + 1
		add = (kz >= 0);
	end

	if add
		kx = kx + kx1;
		ky = ky + ky1;
		kz = kz + kz1;
	else
		kx = kx - kx1;
		ky = ky - ky1;
		kz = kz - kz1;
	end
	nm = norm([kx ky kz]);
	if nm == 0,
		q = Quaternion([1 0 0 0]);
	else
		s = sqrt(1 - qs^2) / nm;
		qv = s*[kx ky kz];

		q = Quaternion([qs qv]);

	end
endfunction


function [ahm,AHM_f,AHM_ahmf] = fromFrameAhm(F,ahmf)

% FROMFRAMEAHM  Transforms AHM from local frame to global frame.
%   AHM = FROMFRAMEAHM(F,AHMF) transforms the Inverse Depth point IF from the
%   local frame F to the global frame. The frame F can be specified either
%   with a 7-vector F=[T;Q], where T is the translation vector and Q the
%   orientation quaternion, of via a structure containing at least the
%   fields F.t, F.q, F.R and F.Rt (translation, quaternion, rotation matrix
%   and its transpose).
%
%   [AHM,AHM_f,AHM_if] = FROMFRAMEAHM(...) returns the Jacobians wrt F and AHMF.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

xf  = ahmf(1:3,:);
mf  = ahmf(4:6,:);
sf  = ahmf(7,:);

[t,q,R] = splitFrame(F);

if nargout == 1

    x  = fromFrame(F,xf);
    m  = R*mf;

    ahm  = [x;m;sf];

else

    if size(ahmf,2) > 1
        error('Jacobians not available for multiple ahms')
    else

        [x, X_f, X_xf] = fromFrame(F,xf);
        [m,M_q,M_mf]   = Rp(q,mf);

        ahm    = [x;m;sf];

        AHM_f = [...
            X_f
            zeros(3,3) M_q
            zeros(1,7)];

        AHM_ahmf = [...
            X_xf          zeros(3,4)
            zeros(3,3) M_mf zeros(3,1)
            zeros(1,6)           1];

    end
end

return

%% jac

syms x y z a b c d X Y Z U V W R real
F   = [x;y;z;a;b;c;d];
ahmf = [X;Y;Z;U;V;W;R];

[ahm,AHM_f,AHM_ahmf] = fromFrameAhm(F,ahmf);

simplify(AHM_f  - jacobian(ahm,F))
simplify(AHM_ahmf - jacobian(ahm,ahmf))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [cout,hout] = tricontour(p,t,Hn,N)
% Contouring for functions defined on triangular meshes
%
%   TRICONTOUR(p,t,F,N)
%
% Draws contours of the surface F, where F is defined on the triangulation
% [p,t]. These inputs define the xy co-ordinates of the points and their
% connectivity:
%
%   P = [x1,y1; x2,y2; etc],            - xy co-ordinates of nodes in the
%                                         triangulation
%   T = [n11,n12,n13; n21,n23,n23; etc] - node numbers in each triangle
%
% The last input N defines the contouring levels. There are several
% options:
%
%   N scalar - N number of equally spaced contours will be drawn
%   N vector - Draws contours at the levels specified in N
%
% A special call with a two element N where both elements are equal draws a
% single contour at that level.
%
%   [C,H] = TRICONTOUR(...)
%
% This syntax can be used to pass the contour matrix C and the contour
% handels H to clabel by adding clabel(c,h) or clabel(c) after the call to
% TRICONTOUR.
%
% TRICONTOUR can also return 3D contours similar to CONTOUR3 by adding
% view(3) after the call to TRICONTOUR.
%
% Type "contourdemo" for some examples.
%
% See also, CONTOUR, CLABEL
% This function does NOT interpolate back onto a Cartesian grid, but
% instead uses the triangulation directly.
%
% If your going to use this inside a loop with the same [p,t] a good
% modification is to make the connectivity "mkcon" once outside the loop
% because usually about 50% of the time is spent in "mkcon".
%
% Darren Engwirda - 2005 (d_engwirda@hotmail.com)
% Updated 15/05/2006
% I/O checking
if nargin~=4
    error('Incorrect number of inputs')
end
if nargout>2
    error('Incorrect number of outputs')
end
% Error checking
if (size(p,2)~=2) || (size(t,2)~=3) || (size(Hn,2)~=1)
    error('Incorrect input dimensions')
end
if size(p,1)~=size(Hn,1)
    error('F and p must be the same length')
end
if (max(t(:))>size(p,1)) || (min(t(:))<=0)
    error('t is not a valid triangulation of p')
end
if (size(N,1)>1) && (size(N,2)>1)
    error('N cannot be a matrix')
end
% Make mesh connectivity data structures (edge based pointers)
[e,eINt,e2t] = mkcon(p,t);
numt = size(t,1);       % Num triangles
nume = size(e,1);       % Num edges
%==========================================================================
%                Quadratic interpolation to centroids
%==========================================================================
% Nodes
t1 = t(:,1); t2 = t(:,2); t3 = t(:,3);
% FORM FEM GRADIENTS
% Evaluate centroidal gradients (piecewise-linear interpolants)
x23 = p(t2,1)-p(t3,1);  y23 = p(t2,2)-p(t3,2);
x21 = p(t2,1)-p(t1,1);  y21 = p(t2,2)-p(t1,2);
% Centroidal values
Htx = (y23.*Hn(t1) + (y21-y23).*Hn(t2) - y21.*Hn(t3)) ./ (x23.*y21-x21.*y23);
Hty = (x23.*Hn(t1) + (x21-x23).*Hn(t2) - x21.*Hn(t3)) ./ (y23.*x21-y21.*x23);
% Form nodal gradients.
% Take the average of the neighbouring centroidal values
Hnx = 0*Hn; Hny = Hnx; count = Hnx;
for k = 1:numt
    % Nodes
    n1 = t1(k); n2 = t2(k); n3 = t3(k);
    % Current values
    Hx = Htx(k); Hy = Hty(k);
    % Average to n1
    Hnx(n1)   = Hnx(n1)+Hx;
    Hny(n1)   = Hny(n1)+Hy;
    count(n1) = count(n1)+1;
    % Average to n2
    Hnx(n2)   = Hnx(n2)+Hx;
    Hny(n2)   = Hny(n2)+Hy;
    count(n2) = count(n2)+1;
    % Average to n3
    Hnx(n3)   = Hnx(n3)+Hx;
    Hny(n3)   = Hny(n3)+Hy;
    count(n3) = count(n3)+1;
end
Hnx = Hnx./count;
Hny = Hny./count;
% Centroids [x,y]
pt = (p(t1,:)+p(t2,:)+p(t3,:))/3;
% Take unweighted average of the linear extrapolation from nodes to centroids
Ht = ( Hn(t1) + (pt(:,1)-p(t1,1)).*Hnx(t1) + (pt(:,2)-p(t1,2)).*Hny(t1) + ...
       Hn(t2) + (pt(:,1)-p(t2,1)).*Hnx(t2) + (pt(:,2)-p(t2,2)).*Hny(t2) + ...
       Hn(t3) + (pt(:,1)-p(t3,1)).*Hnx(t3) + (pt(:,2)-p(t3,2)).*Hny(t3) )/3;
% DEAL WITH CONTOURING LEVELS
if length(N)==1
    lev = linspace(max(Ht),min(Ht),N+1);
    num = N;
else
    if (length(N)==2) && (N(1)==N(2))
        lev = N(1);
        num = 1;
    else
        lev = sort(N);
        num = length(N);
        lev = lev(num:-1:1);
    end
end
% MAIN LOOP
c   = [];
h   = [];
in  = false(numt,1);
vec = 1:numt;
old = in;
for v = 1:num       % Loop over contouring levels

    % Find centroid values >= current level
    i     = vec(Ht>=lev(v));
    i     = i(~old(i));         % Don't need to check triangles from higher levels
    in(i) = true;

    % Locate boundary edges in group
    bnd  = [i; i; i];       % Just to alloc
    next = 1;
    for k = 1:length(i)
        ct    = i(k);
        count = 0;
        for q = 1:3     % Loop through edges in ct
            ce = eINt(ct,q);
            if ~in(e2t(ce,1)) || ((e2t(ce,2)>0)&&~in(e2t(ce,2)))
                bnd(next) = ce;     % Found bnd edge
                next      = next+1;
            else
                count = count+1;    % Count number of non-bnd edges in ct
            end
        end
        if count==3                 % If 3 non-bnd edges ct must be in middle of group
            old(ct) = true;         % & doesn't need to be checked for the next level
        end
    end
    numb = next-1; bnd(next:end) = [];

    % Skip to next lev if empty
    if numb==0
        continue
    end

    % Place nodes approximately on contours by interpolating across bnd
    % edges
    t1  = e2t(bnd,1);
    t2  = e2t(bnd,2);
    ok  = t2>0;

    % Get two points for interpolation. Always use t1 centroid and
    % use t2 centroid for internal edges and bnd midpoint for boundary
    % edges

    % 1st point is always t1 centroid
    H1 = Ht(t1);                                                % Centroid value
    p1 = ( p(t(t1,1),:)+p(t(t1,2),:)+p(t(t1,3),:) )/3;          % Centroid [x,y]

    % 2nd point is either t2 centroid or bnd edge midpoint
    i1        = t2(ok);                                         % Temp indexing
    i2        = bnd(~ok);
    H2        = H1;
    H2(ok)    = Ht(i1);                                         % Centroid values internally
    H2(~ok)   = ( Hn(e(i2,1))+Hn(e(i2,2)) )/2;                  % Edge values at boundary
    p2        = p1;
    p2(ok,:)  = ( p(t(i1,1),:)+p(t(i1,2),:)+p(t(i1,3),:) )/3;   % Centroid [x,y] internally
    p2(~ok,:) = ( p(e(i2,1),:)+p(e(i2,2),:) )/2;                % Edge [x,y] at boundary

    % Linear interpolation
    r     = (lev(v)-H1)./(H2-H1);
    penew = p1 + [r,r].*(p2-p1);

    % Do a temp connection between adjusted node & endpoint nodes in
    % ce so that the connectivity between neighbouring adjusted nodes
    % can be determined
    vecb    = (1:numb)';
    m       = 2*vecb-1;
    c1      = 0*m;
    c2      = 0*m;
    c1(m)   = e(bnd,1);
    c1(m+1) = e(bnd,2);
    c2(m)   = vecb;
    c2(m+1) = vecb;

    % Sort connectivity to place connected edges in sucessive rows
    [c1,i] = sort(c1); c2 = c2(i);

    % Connect adjacent adjusted nodes
    k    = 1;
    next = 1;
    while k<(2*numb)
        if c1(k)==c1(k+1)
            c1(next) = c2(k);
            c2(next) = c2(k+1);
            next     = next+1;
            k        = k+2;         % Skip over connected edge
        else
            k = k+1;                % Node has only 1 connection - will be picked up above
        end
    end
    ncc          = next-1;
    c1(next:end) = [];
    c2(next:end) = [];

    % Plot the contours
    % If an output is required, extra sorting of the
    % contours is necessary for CLABEL to work.
    if nargout>0

        % Form connectivity for the contour, connecting
        % its edges (rows in cc) with its vertices.
        ndx = repmat(1,nume,1);
        n2e = 0*penew;
        for k = 1:ncc
            % Vertices
            n1 = c1(k); n2 = c2(k);
            % Connectivity
            n2e(n1,ndx(n1)) = k; ndx(n1) = ndx(n1)+1;
            n2e(n2,ndx(n2)) = k; ndx(n2) = ndx(n2)+1;
        end
        bndn = n2e(:,2)==0;         % Boundary nodes
        bnde = bndn(c1)|bndn(c2);   % Boundary edges

        % Alloc some space
        tmpv = repmat(0,1,ncc);

        % Loop through the points at the current contour level (lev(v))
        % Try to assemble the CS data structure introduced in "contours.m"
        % so that clabel will work. Assemble CS by "walking" around each
        % subcontour segment contiguously.
        ce    = 1;
        start = ce;
        next  = 2;
        cn    = c2(1);
        flag  = false(ncc,1);
        x     = tmpv; x(1) = penew(c1(ce),1);
        y     = tmpv; y(1) = penew(c1(ce),2);
        for k = 1:ncc

            % Checked this edge
            flag(ce) = true;

            % Add vertices to patch data
            x(next) = penew(cn,1);
            y(next) = penew(cn,2);
            next    = next+1;

            % Find edge (that is not ce) joined to cn
            if ce==n2e(cn,1)
                ce = n2e(cn,2);
            else
                ce = n2e(cn,1);
            end

            % Check the new edge
            if (ce==0)||(ce==start)||(flag(ce))

                % Plot current subcontour as a patch and save handles
                x   = x(1:next-1);
                y   = y(1:next-1);
                z   = repmat(lev(v),1,next);
                h   = [h; patch('Xdata',[x,NaN],'Ydata',[y,NaN],'Zdata',z, ...
                                'Cdata',z,'facecolor','none','edgecolor','flat')]; hold on

                % Update the CS data structure as per "contours.m"
                % so that clabel works
                c = horzcat(c,[lev(v), x; next-1, y]);

                if all(flag)    % No more points at lev(v)
                    break
                else            % More points, but need to start a new subcontour

                    % Find the unflagged edges
                    edges = find(~flag);
                    ce    = edges(1);
                    % Try to select a boundary edge so that we are
                    % not repeatedly running into the boundary
                    for i = 1:length(edges)
                        if bnde(edges(i))
                            ce = edges(i); break
                        end
                    end
                    % Reset counters
                    start = ce;
                    next  = 2;
                    % Get the non bnd node in ce
                    if bndn(c2(ce))
                        cn = c1(ce);
                        % New patch vectors
                        x = tmpv; x(1) = penew(c2(ce),1);
                        y = tmpv; y(1) = penew(c2(ce),2);
                    else
                        cn = c2(ce);
                        % New patch vectors
                        x = tmpv; x(1) = penew(c1(ce),1);
                        y = tmpv; y(1) = penew(c1(ce),2);
                    end

                end

            else
                % Find node (that is not cn) in ce
                if cn==c1(ce)
                    cn = c2(ce);
                else
                    cn = c1(ce);
                end
            end

        end

    else        % Just plot the contours as is, this is faster...

        z = repmat(lev(v),2,ncc);

        patch('Xdata',[penew(c1,1),penew(c2,1)]', ...
              'Ydata',[penew(c1,2),penew(c2,2)]', ...
              'Zdata',z,'Cdata',z,'facecolor','none','edgecolor','flat');

        hold on

    end

end
% Assign outputs if needed
if nargout>0
    cout = c;
    hout = h;
end
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [e,eINt,e2t] = mkcon(p,t)
numt = size(t,1);
vect = 1:numt;
% DETERMINE UNIQUE EDGES IN MESH

e       = [t(:,[1,2]); t(:,[2,3]); t(:,[3,1])];             % Edges - not unique
vec     = (1:size(e,1))';                                   % List of edge numbers
[e,j,j] = unique(sort(e,2),'rows');                         % Unique edges
vec     = vec(j);                                           % Unique edge numbers
eINt    = [vec(vect), vec(vect+numt), vec(vect+2*numt)];    % Unique edges in each triangle
% DETERMINE EDGE TO TRIANGLE CONNECTIVITY
% Each row has two entries corresponding to the triangle numbers
% associated with each edge. Boundary edges have one entry = 0.
nume = size(e,1);
e2t  = repmat(0,nume,2);
ndx  = repmat(1,nume,1);
for k = 1:numt
    % Edge in kth triangle
    e1 = eINt(k,1); e2 = eINt(k,2); e3 = eINt(k,3);
    % Edge 1
    e2t(e1,ndx(e1)) = k; ndx(e1) = ndx(e1)+1;
    % Edge 2
    e2t(e2,ndx(e2)) = k; ndx(e2) = ndx(e2)+1;
    % Edge 3
    e2t(e3,ndx(e3)) = k; ndx(e3) = ndx(e3)+1;
end
return


function RDr=scd_model_GPD_RDr(d,Delta,delta,Dr)
% RDr=scd_model_GPD_RDr(d,Delta,delta,Dr)

alpha=4*delta*Dr/d^2; beta=4*Delta*Dr/d^2;
a=[1.84118378134065	5.33144277352503	8.53631636634628	11.7060049025920	14.8635886339090].^2;% roots of bessel J1'
td=Delta-delta/3;

k=zeros(length(delta),5);
for m=1:5;
    numerator=2*alpha*a(m) - 2 + 2*exp(-alpha*a(m)) + (2-exp(alpha*a(m))-exp(-alpha*a(m))).*exp(-beta*a(m));
    denominator=alpha.^2*a(m).^3.*(a(m)-1);
    k(:,m)=numerator(:)./denominator(:);
end
k(isnan(k))=0;
RDr=sum(k,2).*d.^2./(2*td);


function dice = dice_labels(lname1, lname2)
% d = dice_labels(lname1, lname2)
%
% computes Dice coefficient of the two given label files.
%

%
% dice_labels.m
%
% Original Author: Nick Schmansky
% CVS Revision Info:
%    $Author: nicks $
%    $Date: 2011/03/02 00:04:12 $
%    $Revision: 1.2 $
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

label1 = read_label('',lname1);
label2 = read_label('',lname2);

label1Size = size(label1,1);
label2Size = size(label2,1);

hits = 0;
for i=1:label1Size
   x1 = label1(i,2);
   y1 = label1(i,3);
   z1 = label1(i,4);
   for j=1:label2Size
     x2 = label2(j,2);
     y2 = label2(j,3);
     z2 = label2(j,4);
     if ((x1==x2) && (y1==y2) && (z1==z2))
       hits = hits+1;
       break;
     end
   end
end

dice = (2 * hits) / (label1Size + label2Size);


function a = r8cc_inc ( m, n, nz_num, colptr, rowind, a, i, j, aij )

%*****************************************************************************80
%
%% R8CC_INC increments a value of a R8CC matrix.
%
%  Discussion:
%
%    The R8CC format is the double precision sparse compressed column
%    format.  Associated with this format, we have an M by N matrix
%    with NZ_NUM nonzero entries.  We construct the column pointer
%    vector COL of length N+1, such that entries of column J will be
%    stored in positions COL(J) through COL(J+1)-1.  This indexing
%    refers to both the ROW and A vectors, which store the row indices
%    and the values of the nonzero entries.  The entries of the
%    ROW vector corresponding to each column are assumed to be
%    ascending sorted.
%
%    The R8CC format is equivalent to the MATLAB "sparse" format,
%    and the Harwell Boeing "real unsymmetric assembled" (RUA) format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero entries.
%
%    Input, integer COLPTR(N+1), indicate where each column's data begins.
%
%    Input, integer ROWIND(NZ_NUM), the row indices.
%
%    Input, real A(NZ_NUM), the nonzero entries.
%
%    Input, integer I, J, the indices of the value to retrieve.
%
%    Input, real AIJ, the value to be added to A(I,J).
%
%    Output, real A(NZ_NUM), entry (I,J) has been incremented.
%

%
%  Seek sparse index K corresponding to full index (I,J).
%
  k = r8cc_ijk ( m, n, nz_num, colptr, rowind, i, j );
%
%  If no K was found, we fail.
%
  if ( k == -1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8CC_INC - Fatal error!\n' );
    fprintf ( 1, '  R8CC_IJK could not find the entry.\n' );
    fprintf ( 1, '  Row I = %d\n', i );
    fprintf ( 1, '  Col J = %d\n', j );
    error ( 'R8CC_INC - Fatal error!' );
  end

  a(k) = a(k) + aij;

  return
end


function [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, y, dxs, reg_factor, x_ext)
% SYNTAX:
%   [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, y, dxs, reg_factor, <x_ext>)
%
% EXAMPLE:
%   [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, y, 4, 0, x_ext);
%   [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, [y y_var], 4, 0, x_ext);
%
% INPUT:
%   x            [n x 1] observation time
%   y            [n x 1] observation value
%                [n x 2] observation value and variances (in this case the variances of the data are taken into account)
%   dxs          [1 x 1] spline base size
%   reg_factor   [1 x 1] regularization factor on the first derivative
%   x_ext        [m x 1] points in which to compute the interpolation
%
% OUTPUT:
%   y_splined     [n x 1] interpolated observation (on the observation epochs)
%   x_spline      [o x 1] center of the (o) splines
%   s_weights     [o x 1] weights of the splines
%   y_splined_ext [m x 1] spline interpolated in x_ext positions
%
% DESCRIPTION:
%   Interpolate with cubic splines a given dataset
%

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:        Andrea Gatti
%  Contributors:      Andrea Gatti, Giulio Tagliaferro
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

    if (nargin < 4)
        reg_factor = 0;
    end
    if isempty(x)
        x = 1:size(y, 1);
    end

    inan = isnan(y);
    if (size(y,2) == 2)
        inan = inan(:,1) | inan(:,2);
    end

    x(inan) = [];
    y(inan) = [];

    [x, id] = sort(x);
    y = y(id,:);
    if ~isempty(y)
        if ((nargin == 3) || (reg_factor == 0))
            if (size(y,2) == 2)
                [y_splined, x_spline, s_weights] = spliner_v51(x,y(:,1),y(:,2),dxs);
            else
                [y_splined, x_spline, s_weights] = spliner_v5(x,y,dxs);
            end
        else
            if (size(y,2) == 2)
                [y_splined, x_spline, s_weights] = spliner_v51R(x,y(:,1),y(:,2),dxs, reg_factor);
            else
                [y_splined, x_spline, s_weights] = spliner_v5R(x,y,dxs, reg_factor);
            end
        end
    else
        y_splined = y;
        x_spline = [];
        s_weights = [];
    end

    % Interpolation => using spline to predict in different coordinates
    % (not present in the C version)
    if (nargin == 5)
        if ~isempty(x_spline)
            mask = (isnan(s_weights));
            if (length(mask) > 2)
                mask = mask | [mask(2:end); 0] | [0; mask(1:end-1)];
            end
            s_weights = interp1(x_spline(~mask),s_weights(~mask),x_spline);
            if (size(x_ext,1)==1)
                x_ext = x_ext';
            end
            y_splined_ext = zeros(size(x_ext,1),1);
            for s = 1:length(x_spline)
                tau = round((x_ext-repmat(x_spline(s),length(x_ext),1))/dxs *1e13)/1e13; % 1e13 rounding necessary to avoid numerical problems
                y_splined_ext = y_splined_ext + s_weights(s)*cubicSpline(tau);
            end
        else
            y_splined_ext = nan(numel(x_ext),1);
        end
    else
        y_splined_ext = y_splined;
    end
    tmp = nan(numel(inan),1);
    tmp(~inan) = y_splined;
    y_splined = tmp;
end

% No Regularization + variances
function [y_splined, x_spline, s_weights] = spliner_v51(x, y, y_var, dxs)
    nObs = length(x);

    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil(x_span/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_weights = [];
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    skips = zeros(n_splines,1);
    N = sparse(n_splines,n_splines);
    TN = zeros(n_splines, 1);

    cur_spline = 1;          % first spline whose domain intersect the observation
    %tau = 0;                % normalized distance between the observation and the center of the cur_spline
    i = 1;                  % index of the first observation
    first_obs = i;          % first observation used in the current A matrix
    first_spline = 1;       % first spline used in the current A matrix
    n_skip = 0;              % number of spline to "skip" because ain't intersecting an observation
    used_obs = 0;            % number of observation used in building the N matrix
    y_splined = zeros(length(y),1);           % output
    skips(1) = 0;
    while (i <= nObs)
        % Compute the distance between the current observation and the current spline
        tau = round((x(i)-x_spline(cur_spline))/dxs *1e13)/1e13; % 1e13 rounding necessary to avoid numerical problems
        if (tau <= 2)
            % fill the design matrix
            A(i-first_obs+1,:) = cubicSpline([tau (tau-1) (tau-2) (tau-3)]);
            used_obs = used_obs+1;
            n_skip = 0;
            i = i+1;
        else
            cur_local_spline = cur_spline-first_spline+1;
            skips(cur_spline+1) = i-first_obs;
            % This block of the A matrix is completed
            % Computing N
            n_skip = n_skip +1;
            if (n_skip < 4)
                if (n_skip == 1)
                    A2 = A((skips(cur_spline)+1):i-first_obs,:);
                    iQ = sparse(diag(1./y_var(first_obs+skips(cur_spline):i-1)));
                    N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) = sparse(N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) + A2'*iQ*A2);

                    % Computing TN
                    TN(cur_local_spline:cur_local_spline+3) = TN(cur_local_spline:cur_local_spline+3) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
                end
                cur_spline = cur_spline +1;
            else
                % If I skip more than 3 times the spline solutions are independent,
                % I can start solving my filtering for the first i points

                s_par = [];
                if (used_obs < size(N,2))
                    fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
                    R = sparse(eye(cur_local_spline)*1e-9);
                    s_par = (N(1:cur_local_spline,1:cur_local_spline)+R)\TN(1:cur_local_spline);
                else
                    s_par = (N(1:cur_local_spline,1:cur_local_spline))\TN(1:cur_local_spline);
                end
                s_weights = [s_weights; s_par];

                for s = first_spline:cur_spline-3
                    y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
                end

                % find the next spline whose domain intersect the next observation
                tau = (x(i)-x_spline(cur_spline))/dxs;
                while (tau > 2)
                    cur_spline = cur_spline+1;
                    tau = (x(i)-x_spline(cur_spline))/dxs;
                    if (tau > 2),
                        s_weights(cur_spline) = nan;
                    end
                end
                first_spline = cur_spline;
                first_obs = i;
                skips(cur_spline) = 0;

                A = zeros(nObs-i, 4);
                N = sparse(n_splines-cur_spline+1, n_splines-cur_spline+1);
                TN = zeros(n_splines-cur_spline+1,1);
                used_obs = 0;
            end
        end
    end

    skips(cur_spline+1) = i-first_obs;
    if (n_skip == 0)
        A2 = A(skips(cur_spline)+1:i-first_obs,:);
        iQ = sparse(diag(1./y_var(first_obs+skips(cur_spline):i-1)));
        N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) = sparse(N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2'*iQ*A2);

        % Computing TN
        TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) = TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
    end

    % find the interpolation for the last subset of observations
    s_par = [];
    if (used_obs < size(N,2))
        fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
        R = speye(size(N,2), size(N,2))*1e-9;
        s_par = (N+R)\TN;
    else
        s_par = N\TN;
    end
    s_weights = [s_weights; s_par];

    for s = first_spline:cur_spline
        if ((skips(s)<skips(s+1)))
            y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
        end
    end

    x_spline = x_spline + x0;
end

% Regularization + variances
function [y_splined, x_spline, s_weights] = spliner_v51R(x, y, y_var, dxs, reg_factor)
    nObs = length(x);

    y_var(y_var < reg_factor/2) = reg_factor/2;
    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil(x_span/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_weights = [];
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    skips = zeros(n_splines,1);
    N = sparse(n_splines,n_splines);
    TN = zeros(n_splines, 1);

    cur_spline = 1;          % first spline whose domain intersect the observation
    tau = 0;                % normalized distance between the observation and the center of the cur_spline
    i = 1;                  % index of the first observation
    first_obs = i;          % first observation used in the current A matrix
    first_spline = 1;       % first spline used in the current A matrix
    n_skip = 0;              % number of spline to "skip" because ain't intersecting an observation
    used_obs = 0;            % number of observation used in building the N matrix
    y_splined = zeros(length(y),1);           % output
    skips(1) = 0;
    while (i <= nObs)
        % Compute the distance between the current observation and the current spline
        tau = round((x(i)-x_spline(cur_spline))/dxs *1e13)/1e13; % 1e13 rounding necessary to avoid numerical problems
        if (tau <= 2)
            % fill the design matrix
            A(i-first_obs+1,:) = cubicSpline([tau (tau-1) (tau-2) (tau-3)]);
            used_obs = used_obs+1;
            n_skip = 0;
            i = i+1;
        else
            cur_local_spline = cur_spline-first_spline+1;
            skips(cur_spline+1) = i-first_obs;
            % This block of the A matrix is completed
            % Computing N
            n_skip = n_skip +1;
            if (n_skip < 4)
                if (n_skip == 1)
                    A2 = A((skips(cur_spline)+1):i-first_obs,:);
                    iQ = sparse(diag(1./y_var(first_obs + skips(cur_spline):i-1)));
                    N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) =   sparse(N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) + A2'*iQ*A2);

                    % Computing TN
                    TN(cur_local_spline:cur_local_spline+3) = TN(cur_local_spline:cur_local_spline+3) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
                end
                cur_spline = cur_spline +1;
            else
                % find the next spline whose domain intersect the next observation
                tau = (x(i)-x_spline(cur_spline))/dxs;
                while (tau > 2)
                    cur_spline = cur_spline+1;
                    tau = (x(i)-x_spline(cur_spline))/dxs;
                end
                skips(cur_spline) = skips(cur_spline-1);
                used_obs = -1e10;
            end
        end
    end

    skips(cur_spline+1) = i-first_obs;
    if (n_skip == 0)
        A2 = A(skips(cur_spline)+1:i-first_obs,:);
        iQ = sparse(diag(1./y_var(first_obs+skips(cur_spline):i-1)));
        N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) = sparse(N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2'*iQ*A2);

        % Computing TN
        TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) = TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
    end

    % find the interpolation for the last subset of observations
    s_par = [];
    if (size(N,2)>2)
        if reg_factor == 0
            fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
            reg_factor = 1e-9;
        end
        R = sparse(eye(size(N,2))-diag(ones(size(N,2)-1,1),1)-diag(ones(size(N,2)-1,1),-1) + diag([0; ones(size(N,2)-2,1); 0]));
        R = R*reg_factor;
        s_par = (N+R)\TN;
    else
        if (used_obs < size(N,2))
            if reg_factor == 0
                fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
                reg_factor = 1e-9;
            end
            R = sparse(eye(size(N,2))*reg_factor);
            s_par = (N+R)\TN;
        else
            s_par = N\TN;
        end
    end
    s_weights = [s_weights; s_par];

    for s = first_spline:cur_spline
        if ((skips(s)<skips(s+1)))
            y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
        end
    end

    x_spline = x_spline + x0;
end

% No Regularization - no variances
function [y_splined, x_spline, s_weights] = spliner_v5(x, y, dxs)
    nObs = length(x);

    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil(x_span/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    A_idx = zeros(nObs, 4);

    tau   = round(rem(x',dxs)/dxs*1e13)/1e13;  % 1e13 rounding necessary to avoid numerical problems
    idx   = floor((x')/dxs)+1;
    A     = cubicSpline4Col(tau);
    A_idx = [idx(:) idx(:)+1 idx(:)+2 idx(:)+3];
    n_par = max(A_idx(:,4));
    n_obs = numel(x);
    rows  = repmat((1:n_obs)',1,4);

    A = sparse(rows, A_idx, A, n_obs, n_par);

    idx_null = sum(A~=0) == 0;
    A(:,idx_null) = [];

    N = A'*A;
    B = A'*y;

    x = N\B;

    s_weights = nan(numel(idx_null),1);
    s_weights(~idx_null) = x;
    y_splined = A*x;

    x_spline = x_spline + x0;
end

% Regularization - no variances
function [y_splined, x_spline, s_weights] = spliner_v5R(x, y, dxs, reg_factor)
    nObs = length(x);

    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil((x_span+eps(x_span))/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_weights = [];
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    skips = zeros(n_splines,1);
    N = sparse(n_splines,n_splines);
    TN = zeros(n_splines, 1);

    cur_spline = 1;          % first spline whose domain intersect the observation
    tau = 0;                % normalized distance between the observation and the center of the cur_spline
    i = 1;                  % index of the first observation
    first_obs = i;          % first observation used in the current A matrix
    first_spline = 1;       % first spline used in the current A matrix
    n_skip = 0;              % number of spline to "skip" because ain't intersecting an observation
    used_obs = 0;            % number of observation used in building the N matrix
    y_splined = zeros(length(y),1);           % output
    skips(1) = 0;
    while (i <= nObs)
        % Compute the distance between the current observation and the current spline
        tau = round((x(i) - x_spline(cur_spline))/dxs * 1e13) / 1e13; % 1e13 rounding necessary to avoid numerical problems
        if (tau <= 2)
            % fill the design matrix
            A(i-first_obs+1,:) = cubicSpline([tau (tau-1) (tau-2) (tau-3)]);
            used_obs = used_obs+1;
            n_skip = 0;
            i = i+1;
        else
            cur_local_spline = cur_spline-first_spline+1;
            skips(cur_spline+1) = i-first_obs;
            % This block of the A matrix is completed
            % Computing N
            n_skip = n_skip +1;
            if (n_skip < 4)
                if (n_skip == 1)
                    A2 = A((skips(cur_spline)+1):i-first_obs,:);
                    N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) =   sparse(N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) + A2'*A2);

                    % Computing TN
                    TN(cur_local_spline:cur_local_spline+3) = TN(cur_local_spline:cur_local_spline+3) + A2' * y(first_obs+skips(cur_spline):i-1);
                end
                cur_spline = cur_spline +1;
            else
                % find the next spline whose domain intersect the next observation
                tau = (x(i)-x_spline(cur_spline))/dxs;
                while (tau > 2)
                    cur_spline = cur_spline+1;
                    tau = (x(i)-x_spline(cur_spline))/dxs;
                end
                skips(cur_spline) = skips(cur_spline-1);
                used_obs = -1e10;
            end
        end
    end

    skips(cur_spline+1) = i-first_obs;
    if (n_skip == 0)
        A2 = A(skips(cur_spline)+1:i-first_obs,:);
        N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) = N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2'*A2;

        % Computing TN
        TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) = TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2' * y(first_obs+skips(cur_spline):i-1);
    end

    % find the interpolation for the last subset of observations
    s_par = [];
    if (size(N,2)>2)
        %fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
        R = (speye(size(N,2), size(N,2)) - spdiags(ones(size(N,2), 1), 1, size(N,2), size(N,2)) - spdiags(ones(size(N,2), 1), -1, size(N,2), size(N,2)) + spdiags([0; ones(size(N,2) - 2, 1); 0], 0, size(N,2), size(N,2))) * reg_factor;
        s_par = (N+R)\TN;
    else
        if (used_obs < size(N,2))
            %fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
            R = speye(size(N,2)) * reg_factor;
            s_par = (N+R)\TN;
        else
            s_par = N\TN;
        end
    end
    s_weights = [s_weights; s_par];

    for s = first_spline:cur_spline
        if (skips(s) == 0 && s > 1)
            skips(s) = skips(s-1);
        end
        if ((skips(s)<skips(s+1)))
            y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
        end
    end

    x_spline = x_spline + x0;
end

% SYNTAX:
%   [y] = cubicSpline(t)
%
% EXAMPLE:
%   [y] = cubicSpline(1)
%
% INPUT:
%   t = normalized value from [-2 to 2] of the distance between the
%       observation and the center of the cubic spline
%
% OUTPUT:
%   y = value of the spline in the given point
%
% DESCRIPTION:
%   Get the value of the cubic spline with a base of 4 at the given t normalized
%   value
%
% USEFUL VALUES:
%   2/3 = cubicSpline(0);
%   1/6 = cubicSpline(-1);
%   1/6 = cubicSpline(1);
%
% by Andrea Gatti
%
function [y] = cubicSpline(t)

% FAST VECTORIAL IMPLEMENTATION
y = zeros(size(t));
pos = (t > -2) + (t > -1) - (t > 1) - (t > 2);

% pos = 0    => spline = 0;
% pos = 1    => spline = (-abs(t)+2)^3/6;
% pos = 2    => spline = ((-abs(t)+2)^3 - 4*(-abs(t)+1)^3)/6;

p1 = pos==1;
p2 = pos==2;
t = abs(t);
y(p1) = (2-t(p1)).^3/6;
y(p2) = ((2-t(p2)).^3 - 4*(1-t(p2)).^3)/6;

%  PLAIN IMPLEMENTATION
%
% 	if ((t < -2) || (t > 2))
% 		y = 0;
%     else
%         if (t < 0)
%             if (t <= -1)
%                 tmp2=(t+2);
%                 y = (tmp2*tmp2*tmp2 / 6);
%             else
%                 tmp1=(t+1);
%                 tmp2=(t+2);
%                 y = ((tmp2*tmp2*tmp2 - 4*tmp1*tmp1*tmp1) / 6);
%             end
%         else
%             if (t < 1)
%                 tmp1=(1-t);
%                 tmp2=(2-t);
%                 y = ((tmp2*tmp2*tmp2 - 4*tmp1*tmp1*tmp1) / 6);
%             else
%                 tmp2=(2-t);
%                 y = (tmp2*tmp2*tmp2 / 6);
%             end
%         end
%     end
end

function [val] = cubicSpline4Col(t)
    % Compute matrix entry for cubic spline
    %
    % INPUT
    %   t -> 0 : 1
    %   order -> 1,3
    %
    % SYNTAX:
    %  Core_Utils.cubicSplic(t)
    val = zeros(numel(t),4);
    val(:,1) = (1 - t).^3/6;
    val(:,2) = ((2-t).^3 - 4*(1-t).^3)/6;
    val(:,3) = ((1+t).^3 - 4*(t).^3)/6;
    val(:,4) = (t).^3/6;
end


function [err,time,solver,eqn] = femStokes(node,elem,pde,bdFlag,option,varargin)
%% FEMSTOKES solve the Stokes equation by various finite element methods
%
%   FEMSTOKES computes approximations to the Stokes equation on a
%   sequence of meshes obtained by uniform refinement of a input mesh.
%
% Created by Ming Wang at Nov., 2012.
%
% See also femPoisson femrateStokes
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Default setting of mesh and pde data
if ~exist('node','var') || ~exist('elem','var')
    [node,elem] = squaremesh([0,1,0,1],0.125);  % default mesh is a square
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = Stokesdata1;                          % default data
end
if ~exist('bdFlag','var')
    bdFlag = setboundary(node,elem,'Dirichlet');
end

%% Parameters
option = femoption(option);
maxIt = option.maxIt;   maxN = option.maxN; L0 = option.L0;
option = femStokesoption(option);
elemType = option.elemType; refType = option.refType;

%% Generate an initial mesh
for k = 1:L0
    if strcmp(option.refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(option.refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
end

%% Initialize err
erruH1 = zeros(maxIt,1); errpL2 = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    switch upper(elemType)
        case 'P2P1'
            [u,p,edge,A,eqn,info] = StokesP2P1(node,elem,pde,bdFlag,option);
        case 'P2P0'
            [u,p,edge,A,eqn,info] = StokesP2P0(node,elem,pde,bdFlag,option);
        case 'ISOP2P1'
            [u,p,edge,A,eqn,info] = StokesisoP2P1(node,elem,pde,bdFlag,option);
        case 'ISOP2P0'
            [u,p,edge,A,eqn,info] = StokesisoP2P0(node,elem,pde,bdFlag,option);
        case 'CRP0'
            [u,p,edge,A,eqn,info] = StokesCRP0(node,elem,pde,bdFlag,option);
        case 'CRP1'
            [u,p,edge,A,eqn,info] = StokesCRP1(node,elem,pde,bdFlag,option);
        case 'MINI'
            [u,p,edge,A,eqn,info] = StokesMini(node,elem,pde,bdFlag,option);
        case 'P1BP1'
            [u,p,edge,A,eqn,info] = StokesP1bP1(node,elem,pde,bdFlag,option);
    end
    % compute error
    tic;
%     if strcmp(elemType,'P1BP1')
%         uI = Lagrangeinterpolate(pde.exactu,node,elem);
%     elseif strcmp(elemType(1:2),'CR')
%         uI = Lagrangeinterpolate(pde.exactu,node,elem,'CR',edge);
%     end
    if strcmp(elemType,'P2P0') || strcmp(elemType,'P2P1') || ...
       strcmp(elemType,'isoP2P0') || strcmp(elemType,'isoP2P1')
        uI = pde.exactu([node; (node(edge(:,1),:)+node(edge(:,2),:))/2]);
    elseif strcmp(elemType,'CRP0') || strcmp(elemType,'CRP1')
        uI = pde.exactu((node(edge(:,1),:)+node(edge(:,2),:))/2);
    elseif strcmp(elemType,'Mini')
        uI = pde.exactu(node);
    elseif strcmp(elemType,'P1bP1')
        % bubble part won't be taken in the error computation
        Nv = size(node,1); NT = size(elem,1);
        u0 = pde.exactu(node);
        uI = u;
        uI([(1:Nv)'; NT+Nv+(1:Nv)']) = u0(:);
    end
    erruH1(k) = sqrt((u-uI(:))'*A*(u-uI(:)));
    errpL2(k) = getL2error(node,elem,pde.exactp,p);
    errTime(k) = toc;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
            errTime(k),erruH1(k), errpL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
%    stopErr(k) = info.stopErr;
%    flag(k) = info.flag;
    % plot
    N(k) = size(node,1);
    if option.plotflag && N(k) < 2e3 % show mesh and solution for small size
        if length(p) == size(elem,1) % piecewise constant function
            p = recoverP02P1(node,elem,p);
        end
        figure(1);  showresult(node,elem,p);
    end
    if N(k) > maxN
        break;
    end
    % refine mesh
    tic;
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
    meshTime(k) = toc;
end
h = 1./sqrt(N(1:k));

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrateh(h,erruH1(1:k),1,'k-+','|u_I-u_h|_1');
    subplot(1,2,2)
    showrateh(h,errpL2(1:k),1,'m-+','||p-p_h||');
end

%% Output
err = struct('N',N,'uH1',erruH1(1:k),'pL2',errpL2(1:k));
time = struct('N',N,'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assmble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error
ts = zeros(k,6); ts = char(ts);
display('#nodes     |u_I-u_h|_1      ||p-p_h||  ');
% format shorte
display([num2str(err.N) ts num2str(err.uH1,'%0.5e') ts num2str(err.pL2,'%0.5e')]);


function [oz] = cm32oz(cm3)
% Convert volume from cubic centimeters to US liquid ounces.
% Chad Greene 2012
oz = cm3*0.033814022701;


function [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,options,varargin)
% [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
%    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,varargin)
%
% Backtracking linesearch to satisfy Armijo condition
%
% Inputs:
%   x: starting location
%   t: initial step size
%   d: descent direction
%   f: function value at starting location
%   fr: reference function value (usually funObj(x))
%   gtd: directional derivative at starting location
%   c1: sufficient decrease parameter
%   debug: display debugging information
%   LS_interp: type of interpolation
%   progTol: minimum allowable step length
%   doPlot: do a graphical display of interpolation
%   funObj: objective function
%   varargin: parameters of objective function
%
% Outputs:
%   t: step length
%   f_new: function value at x+t*d
%   g_new: gradient value at x+t*d
%   funEvals: number function evaluations performed by line search
%   H: Hessian at initial guess (only computed if requested)
%
% recet change: LS changed to LS_interp and LS_multi

% Evaluate the Objective and Gradient at the Initial Step
if nargout == 6
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
else

  x_struct = vector2struct(x+t*d, options.TEMPLATE);
  [f_new,g_new_struct] = feval(funObj, x_struct,varargin{:});
  g_new = struct2vector(g_new_struct);

end
funEvals = 1;

while f_new > fr + c1*t*gtd || ~isLegal(f_new)
    temp = t;

    if LS_interp == 0 || ~isLegal(f_new)
        % Ignore value of new point
        if debug
            fprintf('Fixed BT\n');
        end
        t = 0.5*t;
    elseif LS_interp == 1 || ~isLegal(g_new)
        % Use function value at new point, but not its derivative
        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ quadratic interpolation based on two points
            if debug
                fprintf('Quad BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ cubic interpolation based on three points
            if debug
                fprintf('Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1); t_prev f_prev sqrt(-1)],doPlot,0,t);
        end
    else
        % Use function value and derivative at new point

        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ cubic interpolation w/ derivative
            if debug
                fprintf('Grad-Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d],doPlot,0,t);
        elseif ~isLegal(g_prev)
            % Backtracking w/ quartic interpolation 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quartic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ quintic interpolation of 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quintic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev g_prev'*d],doPlot,0,t);
         end
    end

    % Adjust if change in t is too small/large
    if t < temp*1e-3
        if debug
            fprintf('Interpolated Value Too Small, Adjusting\n');
        end
        t = temp*1e-3;
    elseif t > temp*0.6
        if debug
            fprintf('Interpolated Value Too Large, Adjusting\n');
        end
        t = temp*0.6;
    end

    % Store old point if doing three-point interpolation
    if LS_multi
        f_prev = f_new;
        t_prev = temp;
        if LS_interp == 2
            g_prev = g_new;
        end
    end

    if ~saveHessianComp && nargout == 6
        [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    else

      x_struct = vector2struct(x+t*d, options.TEMPLATE);
      [f_new,g_new_struct] = feval(funObj, x_struct,varargin{:});
      g_new = struct2vector(g_new_struct);

%         [f_new,g_new] = funObj(x + t*d,varargin{:});
    end
    funEvals = funEvals+1;

    % Check whether step size has become too small
    if max(abs(t*d)) <= progTol
        if debug
            fprintf('Backtracking Line Search Failed\n');
        end
        t = 0;
        f_new = f;
        g_new = g;
        break;
    end
end

% Evaluate Hessian at new point
if nargout == 6 && funEvals > 1 && saveHessianComp

  [f_new,g_new,H] = funObj(x + t*d,varargin{:});
  funEvals = funEvals+1;

end

x_new = x + t*d;

end


function [Btuph] = hp2Btuph(hp)
% Convert power from mechanical horsepower to British
% thermal units per hour.
% Chad A. Greene 2012
Btuph = hp*2544.433577644;


% system process model in local s0 frame with dcm formulation, Cs0e is the
% transformation between the sensor local s0 frame and e-frame
% written by Huai
% rqs02e, r s0 in e, q s0 2e, rvqs0, rs in s0, vs in s0, qs0 2s
% acc, m/s^2, acc by imu in s frame, gyro, rad/s, angular rate by imu
% w.r.t i frame coordinated in s frame, dt, time interval for covariance update
function [STM Qd]=sys_local_dcm_v000(rqs02e, rvqs0, acc, gyro, dt, imutype, modelNo)
% the covariance corresponds to states, rs0 in e, q s02e,
% rs in s0, v s in s0, q s02s, ba, bg, sa, sg, qs2c, Ts in c,
% for each group frame, qs02si and Tsi in s0, for each
% point, its inverse depth, here we are only interested up to sa and sg
% for modelNo=3 and imutype=5, MEMS
% acc bias drift, random walk
% gyro bias drift, random walk
% acc scale factor, random walk
% gyro scale factor, random walk

% for modelNo=1 and imutype=5 the acc and gyro turn on bias are removed
R=6317000; % earth average radius
%system disturbance coefs
N=zeros(15,6);
N(13:15,4:6)=eye(3); %attitude
Cs02s=quat2dcm_v000(rvqs0(7:10));
N(10:12,1:3)=Cs02s'; %velocity

%system matrix
A=zeros(15);
% 0s for r s0 in e and q s0 to e
A(7:9, 10:12)=eye(3); % rs in s0
%Velocity
xyz_imu=rqs02e(1:3)+quatrot_v000(rqs02e(4:7),rvqs0(1:3),0);
unit=xyz_imu/norm(xyz_imu,2);
Llh=ecef2geo_v000(xyz_imu,0);
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(Llh);
geCoeff=-g*R^2/norm(xyz_imu,2)^3*(eye(3)-3*(unit*unit'))-skew([0;0;WIE_E])*skew([0;0;WIE_E]);
Cs02e=quat2dcm_v000(rqs02e(4:7));
A(10:12,1:3)=Cs02e'*geCoeff;
A(10:12,7:9)=Cs02e'*geCoeff*Cs02e;

Cne=pos2Cne_v000(Llh(1), Llh(2));
ge=Cne(:,3)*g-[xyz_imu(1:2);0]*WIE_E^2; % gravitation in e frame
A(10:12,4:6)=Cs02e'*(-skew(ge)+geCoeff*skew(quatrot_v000(rqs02e(4:7),rvqs0(1:3),0))+...
    skew(skew([0;0;WIE_E])*skew([0;0;WIE_E])*xyz_imu)-2*skew(quatrot_v000(rqs02e(4:7),rvqs0(4:6),0))*skew([0;0;WIE_E]));

A(10:12, 10:12)=-2*skew(quatrot_v000(rqs02e(4:7),[0;0;WIE_E], 1));
A(10:12, 13:15)=-Cs02s'*skew(acc);
%Attitude
A(13:15,4:6)=Cs02s*Cs02e'*skew([0;0;WIE_E]);
A(13:15,13:15)=skew(-gyro);
Anav=A;
Nnav=N;
% X(k+1) = ffun[X(k),U(k),V(k)]
% X(k+1) = Ak*X(k)+Gk*Vk

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);

%%%%Combine and discretize nav and imu models
% this discretization can also be accomplished by Loan's matrix exponential
% method, see sys_metric_phipsi_v000.m
navStates=15;
Anav_d=eye(navStates)+dt*Anav;  %Use 1st order taylor series to discretize Anav
Qnav=Nnav*Rimu*Nnav';
Qnav_d=dt/2*(Anav_d*Qnav+Qnav*Anav_d');      %Use trapezoidal rule to discretize Rimu

STM=zeros(navStates+size(Aimu_d,1));
STM(1:navStates,1:navStates)=Anav_d;
STM(1:navStates,1+navStates:end)=Nnav*Cimu*dt;
STM(1+navStates:end,1+navStates:end)=Aimu_d;

Qd=zeros(navStates+size(Aimu_d,1));
Qd(1:navStates,1:navStates)=Qnav_d;
Qd(1+navStates:end,1+navStates:end)=Qimu_d;
Qd(1:navStates,1+navStates:end)=Nnav*Cimu*Qimu_d*dt/2;
Qd(1+navStates:end,1:navStates)=Qd(1:navStates,1+navStates:end)';
end