function [mask] = polygon_filter(x, y, XI, YI, in_or_out)
    % POLYGON_FILTER used Analytic Geometry to select points inside or outside polygon
    %
    % [mask] = POLYGON_FILTER(x, y, XI, YI, in_or_out)
    %
    % input params:
    %    x, y : polygon vertices, with x(1)==x(end) and y(1)==y(end)
    %           x & y must be vectors of same size, and include
    %           more than 3 points (len(polyX) > 3)
    %
    %    XI, YI : x and y values for points to be tested
    %
    %    in_or_out : either 'inside' or 'outside'
    %               'inside' : returns a mask that is true for points inside polygon
    %               'outside': returns mask that is true for points outside polygon
    %
    %               default is 'inside'
    %
    %    I don't know about edge cases.
    %
    % see also inpolygon, inpoly

    assert(length(XI) == length(YI), 'test-points should have equal numbers of x and y' );
    assert(length(x) == length(y), 'number of polygon x should equal polygon y');
    assert(numel(x) > 3,'not enough points to be a polygon');

    m = length(x)-1;      %  number of coordinates of polygon
    l = zeros(size(XI));
    l2 = l;            %  Algorithm to select points inside a closed
    %  polygon based on Analytic Geometry    R.Z. 4/94
    for i = 1:m

        l= ((y(i)-YI < 0) & (y(i+1)-YI >= 0)) & ...
            (XI-x(i)-(YI-y(i))*(x(i+1)-x(i))/(y(i+1)-y(i)) < 0) | ...
            ((y(i)-YI >= 0) & (y(i+1)-YI < 0)) & ...
            (XI-x(i)-(YI-y(i))*(x(i+1)-x(i))/(y(i+1)-y(i)) < 0);

        if i ~= 1
            l2(l) = 1 - l2(l);
        else
            l2 = l;
        end         % if i

    end

    if ~exist('in_or_out','var')
        in_or_out = 'inside';
    end

    switch(in_or_out)
        case 'inside'
            mask = l2;
        case 'outside'
            mask = ~l2;
        otherwise
            error("unrecognized in_or_out option. either 'inside' or 'outside'");
    end
end


% IM = mkRamp(SIZE, DIRECTION, SLOPE, INTERCEPT, ORIGIN)
%
% Compute a matrix of dimension SIZE (a [Y X] 2-vector, or a scalar)
% containing samples of a ramp function, with given gradient DIRECTION
% (radians, CW from X-axis, default = 0), SLOPE (per pixel, default =
% 1), and a value of INTERCEPT (default = 0) at the ORIGIN (default =
% (size+1)/2, [1 1] = upper left).  All but the first argument are
% optional.

% Eero Simoncelli, 6/96. 2/97: adjusted coordinate system.

function [res] = mkRamp(sz, dir, slope, intercept, origin)

sz = sz(:);
if (size(sz,1) == 1)
  sz = [sz,sz];
end

% -----------------------------------------------------------------
% OPTIONAL args:

if (exist('dir') ~= 1)
  dir = 0;
end

if (exist('slope') ~= 1)
  slope = 1;
end

if (exist('intercept') ~= 1)
  intercept = 0;
end

if (exist('origin') ~= 1)
  origin = (sz+1)/2;
end

% -----------------------------------------------------------------

xinc = slope*cos(dir);
yinc = slope*sin(dir);

[xramp,yramp] = meshgrid( xinc*([1:sz(2)]-origin(2)), ...
    yinc*([1:sz(1)]-origin(1)) );

res = intercept + xramp + yramp;


function result = gaussint(P,a,b,ngp)
% Integration using Gauss Quadratures
% by Manuel Diaz 2013.03.20

% compute gauss locations and weights
[w,psi] = gauss1d(ngp);

% compute the change the integration range to -1 to 1
x = (b+a)/2 + (b-a)/2*psi;

% perform quadrature
result  = (b-a)/2*sum(w.*P(x));


function [orientim, G_xx, G_yy, G_xy, cos_2_theta, sin_2_theta, denom] = orient(or_im, g_sigma, b_sigma, o_smooth)

    % Calculate image gradients.
    hsize = fix(6*g_sigma);
    if ~mod(hsize,2); hsize = hsize+1; end
    f = fspecial('gaussian', hsize, g_sigma);
    [f_x,f_y] = gradient(f);

    G_x = filter2(f_x, or_im);
    G_y = filter2(f_y, or_im);

    G_xx = G_x.^2;
    G_xy = G_x.*G_y;
    G_yy = G_y.^2;

    % to smooth the covariance data

    hsize = fix(6*b_sigma);
    if ~mod(hsize,2); hsize = hsize+1; end
    f = fspecial('gaussian', hsize, b_sigma);

    G_xx = filter2(f, G_xx);
    G_xy = 2*filter2(f, G_xy);
    G_yy = filter2(f, G_yy);

    denom = sqrt(G_xy.^2 + (G_xx - G_yy).^2) + eps;
    sin_2_theta = G_xy./denom;            % Sin of angle
    cos_2_theta = (G_xx-G_yy)./denom;     % Cos of angle

    %Smooth
        hsize = fix(6*o_smooth);
        if ~mod(hsize,2); hsize = hsize+1; end
        f = fspecial('gaussian', hsize, o_smooth);
        cos_2_theta = filter2(f, cos_2_theta);
        sin_2_theta = filter2(f, sin_2_theta);

    a = pi/2;
    orientim = a + atan2(sin_2_theta,cos_2_theta)/2;


rng default;
% Signal space
N = 256;
% Number of measurements
M = 64;
% Sparsity level
K = 10;
% Construct the signal generator.
gen  = spx.data.synthetic.SparseSignalGenerator(N, K);
% Generate bi-uniform signals
x = gen.biUniform(1, 2);
% Sensing matrix
Phi = spx.dict.simple.gaussian_dict(M, N);
% Measurement vectors
y = Phi.apply(x);
% OMP MMV solver instance
result = spx.pursuit.single.omp_chol(double(Phi), y, K, 1e-6);
% Solution vector
z = result.z;

stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
spx.commons.sparse.print_recovery_performance(stats);

mf = spx.graphics.Figures();
mf.new_figure('OMP solution');
subplot(411);
stem(x, '.');
title('Sparse vector');
subplot(412);
stem(z, '.');
title('Recovered sparse vector');
subplot(413);
stem(abs(x - z), '.');
title('Recovery error');
subplot(414);
stem(y, '.');
title('Measurement vector');


function [C,R] = incircle(x,y)
% incircle: compute the maximal in-circle of the polygonal convex hull of a set of points in the plane
%
% [C,R] = incircle(x,y)
%    Called with a pair of vectors of the same length,
%    incircle computes the convex hull in 2-dimensions,
%    then finds the maximal radius in-circle that lies
%    entirely within the polygon of the convex hull.
%
% [C,R] = incircle(xy)
%    Called with a single nx2 array, incircle treats
%    this as a list of points in 2 dimensions. Each row
%    of xy is treated as a single point. The convex hull
%    is computed and the maximal in-circle then computed
%    for the convex hull.
%
% [C,R] = incircle(xy,edges)
%    Called with two arguments, the first of which is an
%    nx2 array of data points, and the second a list of
%    edges of the convex hull as would be generated by
%    convhull (or convhulln), the maximal in-circle is
%    generated for the hull as provided. No test is made
%    to assure the hull is truly convex.
%
% All input forms return a row vector of length 2 (C)
% which defines the center of the in-circle, and the
% radius (R) of the in-circle.
%
% Example:
%
% [C,R] = incircle(rand(1000,1),rand(1000,1))
%
% C =
%      0.50191      0.49999
%
% R =
%      0.49788
%
% Example:
%
% x = randn(100,1) + 1;
% y = randn(100,1) - 2.5;
% H = convhull(x,y);
% [C,R] = incircle([x,y],H)
%
% C =
%      1.0212      -2.3458
%
% R =
%      2.1329
%
% t = linspace(0,2*pi,200);
% xc = R*cos(t) + C(1);
% yc = R*sin(t) + C(2);
% plot(x(H),y(H),'-r',x,y,'ko',xc,yc,'-b')
% axis equal
% xlabel X
% ylabel Y
% title 'Convex hull, with polygon in-circle'
% grid on
%
%
% See also: minboundcicle, convhull
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 6/14/09

if (nargin < 1) || (nargin > 2)
  error('INCIRCLE:improperarguments', ...
    'incircle requires exactly 1 or 2 arguments')
elseif (nargin == 2) && isvector(x) && isvector(y) && (numel(x) == numel(y))
  % a pair of vectors
  xy = [x(:),y(:)];
  % compute the hull. I prefer convhulln.
  edges = convhulln(xy);
elseif (nargin == 1) && (size(x,2) == 2)
  % a single list of points as rows of x
  xy = x;
  edges = convhulln(xy);
elseif (nargin == 2) && (size(x,2) == 2) && (size(y,2) == 2)
  % y must be a list of edges from convhulln
  xy = x;
  edges = y;
elseif (nargin == 2) && (size(x,2) == 2) && isvector(y) && (y(1) == y(end))
  % y must be a list of edges from convhull
  xy = x;
  I = y(:);
  % in case the first point was not wrapped in the polygon
  if I(1) ~= I(end)
    I = [I;I(1)];
  end
  edges = [I(1:(end-1)),I(2:end)];
else
  % none of the forms I allow seem to fit.
  % give up and throw an error.
  error('INCIRCLE:invaliddata', ...
    'x and y do not seem to fit into any of the allowed forms for incircle input')
end
ne = size(edges,1);

% the end points of each edge are...
A = xy(edges(:,1),:);
B = xy(edges(:,2),:);

% the normal vector to each edge
N = (B - A)*[0 1;-1 0];

% normalize to unit length
L = sqrt(sum(N.^2,2));
N = N./[L,L];

% a central point inside the hull itself
C0 = mean(A,1);

% ensure the normals point inwards. While
% I can use the signed area for a hull as
% generated by convhull (suggestion by Bruno)
% this test is also efficient, and it deals
% with the case of a hull provided from some
% unknown and less trusted source.
k = sum(N.*bsxfun(@minus,C0,A),2) < 0;
N(k,:) = -N(k,:);

% formulate the linear programming problem.
% given a point C inside the hull, the distance
% from C to the edge that contains A(i,:) is
% given by (dot(N(i,:),C - A(i,:))). If this
% number is negative for any edge of the hull,
% then C is outside of the hull.
%
% Now think of it as a set of slack variables,
% one for each edge of the hull. Given the
% unknown point C that defines the center of
% the in-circle,
%
%  dot(N(i,:),C-A(i,:)) - S(i) == 0
%
% Thus the vector S is of length ne, where ne is
% the number of edges in the convex hull. Every
% element of S must be positive.
%
% Create one more slack variable, a scalar R.
% R is the minimum value that S attains for a
% given point C.
%
%   R >= 0
%   S(i) >= R
%
% The objective for our linear programming problem
% is simple. It is just -R. When we find the
% solution to the LP problem that satisfies the
% equality constraints above between C and S, the
% bound constraint on T, and the inequality
% constraints between S and R, the solution yields
% the maximal radius in-circle that fits inside
% the convex hull polygon.
%
% The unknowns for the LP are a list of ne+3
% variables. The first two variables represent
% the center coordinates of the circle. X(3) = R
% is the circle radius. The remainder of the
% variables are the slack variabls S(i).

% equality constraints defined by the dot products
% with the normal vectors.
Aeq = [N,zeros(ne,1),-eye(ne)];
beq = sum(N.*A,2);

% lower bounds only for the slack variables
LB = [-inf; -inf; 0; zeros(ne,1)];
% there are no upper bounds
UB = [];

% inequalities defined by the slack variable
% constraints
A = [zeros(ne,2),ones(ne,1),-eye(ne)];
b = zeros(ne,1);

% the objective is just -R
f = zeros(ne+3,1);
f(3) = -1;

% just turn off the output message
options = optimset('linprog');
options.Display = 'off';

% linprog does all of the hard work.
result = linprog(f,A,b,Aeq,beq,LB,UB,[],options);

% unpack the circle parameters
C = result(1:2)';
R = result(3);


function bitPos=findPosOfMin1Bit(n)
%%FINDPOSOFMIN1BIT Given n, which is of an integer data type, determine the
%           position of the least significant bit that is a 1.
%
%INPUTS: n An integer or a matrix of integers. These must be an integer
%          data type (i.e. not a double).
%
%OUTPUTS: bitPos The position of the first 1 in the binary representation
%                of each of the integers in n. Counting starts at 1. A
%                value of 0 means that there is no 1 in the binary
%                representation.
%
%The algorithm just keeps shifting the number (dividing by 2) until it
%the first digit is no longer 0, counting the number of shifts needed.
%
%EXAMPLE:
% n=findPosOfMin1Bit(int32([0,4,1213,2^23]))
%
%The function returns n=[0, 3, 1, 24]. Binary representations of
%the numbers (the least significant bit is to the left) are
%0    0
%4    001
%1213 10111101001
%2^23 000000000000000000000001
%
%January 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(isfloat(n)||~isreal(n))
    error('The values must be real and be an integer data type.')
end

if(ischar(n))
    n=uint8(n);
end

bitPos=ones(size(n));
numEls=numel(n);

for curEl=1:numEls
    if(n(curEl)~=0)
        while(bitand(n(curEl),1)==0)
            bitPos(curEl)=bitPos(curEl)+1;
            n(curEl)=bitshift(n(curEl),-1);
        end
    else
        bitPos(curEl)=0;
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function x_num = box_behnken_size ( dim_num )

%*****************************************************************************80
%
%% BOX_BEHNKEN_SIZE returns the size of a Box-Behnken design.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Box, Donald Behnken,
%    Some new three level designs for the study of quantitative variables,
%    Technometrics,
%    Volume 2, pages 455-475, 1960.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, integer X_NUM, the number of elements of the design.
%    X_NUM will be equal to DIM_NUM * 2**(DIM_NUM-1) + 1.
%
  if ( 1 <= dim_num )
    x_num = 1 + dim_num * 2^( dim_num - 1 );
  else
    x_num = -1;
  end

  return
end


function fx = ln_cordic ( x, n )

%*****************************************************************************80
%
%% LN_CORDIC evaluates the natural logarithm function using the CORDIC method.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frederick Ruckdeschel,
%    BASIC Scientific Subroutines,
%    Volume II,
%    McGraw-Hill, 1980,
%    ISBN: 0-07-054202-3,
%    LC: QA76.95.R82.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Input, integer N, the number of steps to take.
%
%    Output, real FX, the logarithm of X.
%
%  Local Parameters:
%
%    Local, real A(1:25) = exp ( (1/2)^(1:25) );
%
  a_length = 25;

  a = [ ...
    1.648721270700128, ...
    1.284025416687742, ...
    1.133148453066826, ...
    1.064494458917859, ...
    1.031743407499103, ...
    1.015747708586686, ...
    1.007843097206488, ...
    1.003913889338348, ...
    1.001955033591003, ...
    1.000977039492417, ...
    1.000488400478694, ...
    1.000244170429748, ...
    1.000122077763384, ...
    1.000061037018933, ...
    1.000030518043791, ...
    1.0000152589054785, ...
    1.0000076294236351, ...
    1.0000038147045416, ...
    1.0000019073504518, ...
    1.0000009536747712, ...
    1.0000004768372719, ...
    1.0000002384186075, ...
    1.0000001192092967, ...
    1.0000000596046466, ...
    1.0000000298023228 ];
  e = 2.718281828459045;

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LN_CORDIC - Fatal error!\n' );
    fprintf ( 1, '  Input argument X <= 0.0\n' );
    error ( 'LN_CORDIC - Fatal error!' );
  end

  k = 0;

  while ( e <= x )
    k = k + 1;
    x = x / e;
  end

  while ( x < 1.0 )
    k = k - 1;
    x = x * e;
  end
%
%  Determine the weights.
%
  for i = 1 : n

    w(i) = 0.0;

    if ( i <= a_length )
      ai = a(i);
    else
      ai = 1.0 + ( ai - 1.0 ) / 2.0;
    end

    if ( ai < x )
      w(i) = 1.0;
      x = x / ai;
    end

  end

  x = x - 1.0;
  x = x ...
    * ( 1.0 - ( x / 2.0 ) ...
    * ( 1.0 + ( x / 3.0 ) ...
    * ( 1.0 -   x / 4.0 )));
%
%  Assemble.
%
  poweroftwo = 0.5;
  for i = 1 : n
    x = x + w(i) * poweroftwo;
    poweroftwo = poweroftwo / 2.0;
  end

  fx = k + x;

  return
end


function [ a, more ] = tree_rb_lex_next ( n, a, more )

%*****************************************************************************80
%
%% TREE_RB_LEX_NEXT generates rooted binary trees in lexicographic order.
%
%  Discussion:
%
%    The information definining the tree of N nodes is stored in a vector
%    of 0's and 1's, in preorder traversal form.  Essentially, the
%    shape of the tree is traced out with a pencil that starts at the root,
%    and stops at the very last null leaf.  The first time that a (non-null)
%    node is encountered, a 1 is added to the vector, and the left
%    descendant of the node is visited next.  When the path returns from
%    the first descendant, the second descendant is visited.  When then path
%    returns again, the path passes back up from the node to its parent.
%    A null leaf is encountered only once, and causes a zero to be added to
%    the vector, and the path goes back up to the parent node.
%
%    The lexicographic order is used to order the vectors of 1's and 0's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Reference:
%
%    Frank Ruskey,
%    Combinatorial Generation,
%    To appear.
%
%  Parameters:
%
%    Input, integer N, the number of nodes in the rooted binary
%    tree.  N should be odd.
%
%    Input/output, integer A(N), the preorder traversal form for
%    the previous/next rooted binary tree.
%
%    Output, logical MORE, is TRUE if the next rooted binary tree was
%    returned on this call, or FALSE if there are no more rooted binary
%    trees, and the output of the previous call was the last one.
%
  if ( ~ more )
    a(1:2:n-2) = 1;
    a(2:2:n-1) = 0;
    a(n) = 0;
    more = 1;
    return
  end
%
%  Find the last 1 in A.
%
  k = n;
  while ( a(k) == 0 )
    k = k - 1;
  end
  q = n - k - 1;
%
%  Find the last 0 preceding the last 1 in A.
%  If there is none, then we are done, because 11...1100..00
%  is the final element.
%
  while ( 1 )

    if ( k == 1 )
      more = 0;
      return
    end

    if ( a(k) == 0 )
      break
    end

    k = k - 1;

  end

  p = n - k - q - 1;
  a(k) = 1;
  a(k+1:n-2*p+1) = 0;
  a(n-2*p+2:2:n-2) = 1;
  a(n-2*p+3:2:n-1) = 0;
  a(n) = 0;

  return
end


%DECOMPOSEESSENTIALMAT  Decompose an essential matrix to possible rotations and translation
%
%     S = cv.decomposeEssentialMat(E)
%
% ## Input
% * __E__ The input essential matrix, 3x3.
%
% ## Output
% * __S__ Decomposed `E`. A scalar struct with the following fields:
%   * __R1__ One possible rotation matrix, 3x3.
%   * __R2__ Another possible rotation matrix, 3x3.
%   * __t__ One possible translation, 3x1.
%
% This function decompose an essential matrix `E` using SVD decomposition
% [HartleyZ00]. Generally 4 possible poses exists for a given `E`. They are
% `[R1,t]`, `[R1,-t]`, `[R2,t]`, `[R2,-t]`. By decomposing `E`, you can only
% get the direction of the translation, so the function returns unit `t`.
%
% ## References
% [HartleyZ00]:
% > Richard Hartley and Andrew Zisserman. "Multiple view geometry in computer
% > vision". Cambridge university press, 2003.
%
% See also: cv.findEssentialMat, cv.SVD
%


function [U_exact] = exact_translating_sphere(sphere_translational_velocity,sphere_radius,mesh_eval)
%+========================================================================+
%|                                                                        |
%|           This function uses the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT :                                                            |
%| PROPERTY  :                                                            |
%| LICENCE   :                                                            |
%| CONTACT   :                                                            |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : exact_translating_sphere.m                    |
%|    #    |   VERSION    : 0.10                                          |
%|   _#_   |   AUTHOR(S)  : Luca Berti                                    |
%|  ( # )  |   CREATION   : 25.12.2018                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+
% COMPUTE THE EXACT VELOCITY FIELD DETERMINED BY A TRANSLATING SPHERE IN
% AN INFINITE FLUID ALONG THE Z AXIS
% Reference: Happel-Brenner, "Low Reynolds number hydrodynamics"
% (pag. 119 and formulas 4-17.17 and 4-17.18) transformed into Cartesian
% coordinates (p.507 A-15.29 for the transformation)

% Input -> mesh_eval: mesh containing the evaluation points for the exact
%                     solution
%          sphere_translational_velocity -> translational velocity of the
%                                           vertically moving sphere
%                                           [0,0,W]
% Output -> U_exact:  3xN_eval vector containing the values of the exact
%                     solution at the evaluation point, expressed IN CARTESIAN
%                     COORDINATES

W = sphere_translational_velocity(3);
% Exact solutions taken from the reference - SPHERICAL COORDINATES
u_r = @(x,y,z) -W.*cos(acos(z./sqrt(x.^2+y.^2+z.^2))).*(sphere_radius^3./(2*sqrt(x.^2+y.^2+z.^2).^3) -3*sphere_radius./(2*sqrt(x.^2+y.^2+z.^2)));
u_theta = @(x,y,z) -W.*sin(acos(z./sqrt(x.^2+y.^2+z.^2))).*(sphere_radius^3./(4*sqrt(x.^2+y.^2+z.^2).^3)+3*sphere_radius./(4*sqrt(x.^2+y.^2+z.^2)));
u_phi =@(x,y,z) 0;

X = mesh_eval.vtx;
N_eval = size(X,1);

% Evaluation vector - SPHERICAL COORDINATES
U_exact = zeros(N_eval*3,1);
U_exact(1:N_eval,1) = feval(u_r,X(:,1),X(:,2),X(:,3));
U_exact(N_eval+1:2*N_eval,1) = feval(u_theta,X(:,1),X(:,2),X(:,3));
U_exact(2*N_eval+1:end,1) = feval(u_phi,X(:,1),X(:,2),X(:,3));

% Expression of the evaluation points in spherical coordinates, to extract
% (r,theta,phi) needed to transform the exact velocities from spherical to
% Cartesian coordinates
Y = X;
[X(:,3),X(:,2),X(:,1)] = cart2sph(Y(:,1),Y(:,2),Y(:,3));
% Latitude is needed instead of colatitude (as computed by Matlab)
X(:,2) = pi/2-X(:,2);

temporary = zeros(N_eval*3,1);
% Transformation of the exact velocity vector field from spherical to
% Cartesian coordinates - CARTESIAN COORDINATES
temporary(1:N_eval,1) = U_exact(1:N_eval,1).*sin(X(:,2)).*cos(X(:,3)) + ...
                          U_exact(N_eval+1:N_eval*2,1).*cos(X(:,2)).*cos(X(:,3))- ...
                          U_exact(1+2*N_eval:end,1).*sin(X(:,3));

temporary(1+N_eval:2*N_eval,1) = U_exact(1:N_eval,1).*sin(X(:,2)).*sin(X(:,3)) + ...
                          U_exact(N_eval+1:N_eval*2,1).*cos(X(:,2)).*sin(X(:,3)) + ...
                          U_exact(1+2*N_eval:end,1).*cos(X(:,3));

temporary(1+2*N_eval:end,1) = U_exact(1:N_eval,1).*cos(X(:,2)) - ...
                          U_exact(N_eval+1:N_eval*2,1).*sin(X(:,2));
U_exact = temporary;

end


function X_zero=assignPerimeterPositions(radius,mesh);
% Define a set of perimeter points on the circle.
%
%   X_zero=assignPerimeterPositions(perimeterDists,mesh);
%
% orderMeshPerimeterPointsAll is the routine that gets the perimeter points
% into the proper order for the mapping here.   The mapping here simply
% puts these points on a circle.  In principle, we could put the points at
% these angles but adjust for distance.  In that case, however, we might
% not have a convex set.  So, we use a circle.
%
% The most anterior point in the sub mesh should map to the top of the
% circle. The most posterior point should be on the left or right.
%
% Stanford.

numPerimPoints=length(mesh.orderedUniquePerimeterPoints);

% Find the maximum y value in the perimeter
[maxHeight,maxHIndex]=max(mesh.uniqueVertices(mesh.orderedUniquePerimeterPoints,2));

angles=linspace(0,2*pi,numPerimPoints)';
angles=shift(angles(:),[-maxHIndex,0]);

X_zero=zeros(numPerimPoints,2);

% Perimeter must be convex for Tutte's algorithm  to work properly -
X_zero(:,1)=radius.*cos(angles);
X_zero(:,2)=radius.*sin(angles);

return;


function value = p18_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P18_F evaluates the integrand for problem 18.
%
%  Discussion:
%
%    This is the characteristic function of the interior of the
%    N sphere of radius R and center Z, to be integrated within the
%    unit hypercube [0,1]^N.  If the user picks a combination of R
%    and Z that causes the volume of the sphere to lie at least
%    partially outside the unit hypercube, the formula for the
%    exact integral will no longer be correct.
%
%  Dimension:
%
%    DIM_NUM is arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    R defaults to 0.50.
%    You can change R by calling P18_R8.
%
%    Z(1:DIM_NUM) defaults to (0.5,0.5,...0.5).
%    You can change Z by calling P18_R8VEC.
%
%  Integrand:
%
%    f(x) = 1 if X(1:DIM_NUM) is less than R from Z(1:DIM_NUM),
%           0 otherwise.
%
%  Exact Integral:
%
%    The volume of the DIM_NUM sphere of radius R.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  z = [];
  z = p18_r8vec ( 'G', 'Z', dim_num, z );

  r = 0.0;
  r = p18_r8 ( 'G', 'R', r );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    d = sqrt ( sum ( ( x(1:dim_num,point) - z(1:dim_num)' ).^2 ) );

    if ( d <= r )
      value(point) = 1.0;
    else
      value(point) = 0.0;
    end

  end

  p18_i4 ( 'I', '#', point_num );

  return
end


function itree = pruefer_to_tree_2 ( nnode, iarray )

%*****************************************************************************80
%
%% PRUEFER_TO_TREE_2 produces the edge list of a tree from its Pruefer code.
%
%  Discussion:
%
%    One can thus exhibit all trees on N nodes, produce
%    one at random, find the M-th one on the list, etc, by
%    manipulating the Pruefer codes.
%
%    For every labeled tree on N nodes, there is a unique N-2 tuple
%    of integers A1 through AN-2, with each A between 1 and N.  There
%    are N^(N-2) such sequences, and each one is associated with exactly
%    one tree.
%
%    This routine apparently assumes that the Pruefer code is
%    generated by taking the LOWEST labeled terminal node each time.
%    This is not consistent with PRUEFER_TO_TREE and TREE_TO_PRUEFER.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis, Herbert Wilf.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Albert Nijenhuis. Herbert Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer NNODE, number of nodes in desired tree.
%
%    Input, integer IARRAY(NNODE).  IARRAY(I), I = 1, NNODE-2
%    is the Pruefer code for the tree.
%
%    Output, integer ITREE(NNODE); the I-th edge of the tree
%    joins nodes I and ITREE(I).
%
  itree(1:nnode) = 0;

  for i = nnode-2 : -1 : 1

    l = iarray(i);

    if ( itree(l) == 0 )
      iarray(i) = - l;
      itree(l) = - 1;
    end

  end

  iarray(nnode-1) = nnode;
%
%  Find next index K so that ITREE(K) is 0.
%
  k = 1;
  j = 0;

  while ( itree(k) ~= 0 )
    k = k + 1;
  end

  kp = k;

  while ( 1 )

    j = j + 1;
    ir = abs ( iarray(j) );
    itree(kp) = ir;

    if ( j == nnode - 1 )
      break
    end

    if ( 0 < iarray(j) )
      while ( itree(k) ~= 0 )
        k = k + 1;
      end
      kp = k;
      continue
    end

    if ( k < ir )
      itree(ir) = 0;
      while ( itree(k) ~= 0 )
        k = k + 1;
      end
      kp = k;
      continue
    end

    kp = ir;

  end
%
%  Restore the signs of IARRAY.
%
  iarray(1:nnode-2) = abs ( iarray(1:nnode-2) );

  return
end


function [lmval,indd]=lmax(xx,filt)
%LMAX 	[lmval, indd]=lmax(xx,filt). Find local maxima in vector XX,where
%	LMVAL is the output vector with maxima values, INDD  is the
%	corresponding indexes, FILT is the number of passes of the small
%	running average filter in order to get rid of small peaks.  Default
%	value FILT =0 (no filtering). FILT in the range from 1 to 3 is
%	usially sufficient to remove most of a small peaks
%	For example:
%	xx=0:0.01:35; y=sin(xx) + cos(xx ./3);
%	plot(xx,y); grid; hold on;
%	[b,a]=lmax(y,2)
%	 plot(xx(a),y(a),'r+')
%	see also LMIN, MAX, MIN

%**************************************************|
% 	Serge Koptenko, Guigne International Ltd., |
%	phone (709)895-3819, fax (709)895-3822     |
%--------------06/03/97----------------------------|

x=xx;
len_x = length(x);
	fltr=[1 1 1]/3;
  if nargin <2, filt=0;
	else
x1=x(1); x2=x(len_x);
	for jj=1:filt,
	c=conv(fltr,x);
	x=c(2:len_x+1);
	x(1)=x1;
        x(len_x)=x2;
	end
  end
lmval=[]; indd=[];
i=2;		% start at second data point in time series
    while i < len_x-1,
	if x(i) > x(i-1)
	   if x(i) > x(i+1)	% definite max
lmval =[lmval x(i)];
indd = [ indd i];
	   elseif x(i)==x(i+1)&x(i)==x(i+2)	% 'long' flat spot
%lmval =[lmval x(i)];  	%1   comment these two lines for strict case
%indd = [ indd i];	%2 when only  definite max included
i = i + 2;  		% skip 2 points
	   elseif x(i)==x(i+1)	% 'short' flat spot
%lmval =[lmval x(i)];	%1   comment these two lines for strict case
%indd = [ indd i];	%2 when only  definite max included
i = i + 1;		% skip one point
	   end
	end
	i = i + 1;
    end
if filt>0 & ~isempty(indd),
	if (indd(1)<= 3)|(indd(length(indd))+2>length(xx)),
	   rng=1;	%check if index too close to the edge
	else rng=2;
	end
	  for ii=1:length(indd), 	% Find the real maximum value
	    [val(ii) iind(ii)] = max(xx(indd(ii) -rng:indd(ii) +rng));
	    iind(ii)=indd(ii) + iind(ii)  -rng-1;
	  end
  indd=iind; lmval=val;
else
end


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%
for i = 1:size(X,1)
	min = Inf;
	for j = 1:K
		diff = sum((X(i,:) - centroids(j,:)).^2);
		if min > diff
			min = diff;
			idx(i) = j;
		end
	end
end

% =============================================================

end


function [upsilonaa, upsilonap] = lfmaaComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode)

% LFMAACOMPUTEUPSILONMATRIX Upsilon matrix acce. accel. with t1, t2 limits
% FORMAT
% DESC computes a portion of the LFM kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

sigma = sqrt(sigma2);
gridt1 = repmat(t1, 1, length(t2));
gridt2 = repmat(t2', length(t1), 1);
timeGrid = gridt1 - gridt2;

if mode==0
    if nargout > 1
        upsilonap = lfmapComputeUpsilonMatrix(gamma, sigma2, t1, t2,1);
        upsilonaa = gamma^2*upsilonap + (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).* ...
            ((gamma + 2*timeGrid/sigma2).*(2/sigma2 - (2*timeGrid/sigma2).^2) + 8*timeGrid/sigma^4);
    else
        upsilonaa = gamma^2*lfmapComputeUpsilonMatrix(gamma, sigma2, t1, t2,1) ...
            + (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).* ...
            ((gamma + 2*timeGrid/sigma2).*(2/sigma2 - (2*timeGrid/sigma2).^2) + 8*timeGrid/sigma^4);
    end
else
    if nargout > 1
        upsilonap = lfmapComputeUpsilonMatrix(gamma, sigma2, t1, t2, 0);
        upsilonaa = gamma^2*upsilonap + (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).* ...
            ((gamma + 2*timeGrid/sigma2).*(2/sigma2 - (2*timeGrid/sigma2).^2) + 8*timeGrid/sigma^4) ...
            + (2*gamma^2/(sqrt(pi)*sigma))*exp(-gamma*t1)*((gamma - 2*t2/sigma2).*exp(-(t2.^2)/sigma2)).';

    else
        upsilonaa = gamma^2*lfmapComputeUpsilonMatrix(gamma, sigma2, t1, t2, 0) ...
            + (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).* ...
            ((gamma + 2*timeGrid/sigma2).*(2/sigma2 - (2*timeGrid/sigma2).^2) + 8*timeGrid/sigma^4) ...
            + (2*gamma^2/(sqrt(pi)*sigma))*exp(-gamma*t1)*((gamma - 2*t2/sigma2).*exp(-(t2.^2)/sigma2)).';
    end
end


function [ x, seed ] = burr_sample ( a, b, c, d, seed )

%*****************************************************************************80
%
%% BURR_SAMPLE samples the Burr PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, D, the parameters of the PDF.
%    0 < B,
%    0 < C.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ cdf, seed ] = r8_uniform_01 ( seed );

  x = burr_cdf_inv ( cdf, a, b, c, d );

  return
end


function [lambda_vec, error_train, error_val] = ...
    validationCurve(X, y, Xval, yval)
%VALIDATIONCURVE Generate the train and validation errors needed to
%plot a validation curve that we can use to select lambda
%   [lambda_vec, error_train, error_val] = ...
%       VALIDATIONCURVE(X, y, Xval, yval) returns the train
%       and validation errors (in error_train, error_val)
%       for different values of lambda. You are given the training set (X,
%       y) and validation set (Xval, yval).
%

% Selected values of lambda (you should not change this)
lambda_vec = [0 0.001 0.003 0.01 0.03 0.1 0.3 1 3 10]';

% You need to return these variables correctly.
error_train = zeros(length(lambda_vec), 1);
error_val = zeros(length(lambda_vec), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the validation errors in error_val. The
%               vector lambda_vec contains the different lambda parameters
%               to use for each calculation of the errors, i.e,
%               error_train(i), and error_val(i) should give
%               you the errors obtained after training with
%               lambda = lambda_vec(i)
%
% Note: You can loop over lambda_vec with the following:
%
%       for i = 1:length(lambda_vec)
%           lambda = lambda_vec(i);
%           % Compute train / val errors when training linear
%           % regression with regularization parameter lambda
%           % You should store the result in error_train(i)
%           % and error_val(i)
%           ....
%
%       end
%
%

for i = 1:length(lambda_vec)
    lambda = lambda_vec(i);

    theta = trainLinearReg(X, y, lambda);

    error_train(i) = linearRegCostFunction(X, y, theta, 0);
    error_val(i) = linearRegCostFunction(Xval, yval, theta, 0);
end

% =========================================================================

end

github-repos/MATLAB/Borye-machine-learning-coursera-1/machine-learning-coursera-1-033fdc2e6da393eeb1179a09aafe92362021effb/Week 6 Assignments/Regularized Linear Regression and Bias,Variance/mlclass-ex5-005/mlclass-ex5/validationCurve.m


function yi=akimai(x,y,xi)
%
% Usage: yi=akimai(x,y,xi)
%
%        Given vectors x and y (of the same length)
%        and the array xi at which to interpolate,
%        fits piecewise cubic polynomials and returns
%        the interpolated values yi at xi.
%
% Ref. : Hiroshi Akima, Journal of the ACM, Vol. 17, No. 4, October 1970,
%        pages 589-602.
%
% Programmer: N. Shamsundar, University of Houston, 6/2002
%   Correction to lines 32-33, 9/2004,  motivated by Gilford Ward,
%     to make routine work correctly for linear data.
%
% Notes: Use only for precise data, as the fitted curve passes through the
%        given points exactly. This routine is useful for plotting a pleasingly
%        smooth curve through a few given points for purposes of plotting.
%
x=x(:); y=y(:); xi=xi(:); n=length(x);
if n~=length(y), error('input x and y arrays must be of same length'), end
dx=diff(x); if any(dx <= 0) error('input x-array must be in strictly ascending order'), end
if any(xi<x(1)) | any(xi > x(n))
  warning('All interpolation points xi must lie between x(1) and x(n)')
end
m=diff(y)./dx;
mm=2*m(1)-m(2);     mmm=2*mm-m(1);     % augment at left
mp=2*m(n-1)-m(n-2); mpp=2*mp-m(n-1);   % augment at right
m1=[mmm; mm; m; mp; mpp];              % slopes
dm=abs(diff(m1)); f1=dm(3:n+2); f2=dm(1:n); f12=f1+f2;
id=find(f12 > 1e-8*max(f12)); b=m1(2:n+1);
b(id)=(f1(id).*m1(id+1)+f2(id).*m1(id+2))./f12(id);
c=(3*m-2*b(1:n-1)-b(2:n))./dx;
d=(b(1:n-1)+b(2:n)-2*m)./dx.^2;

% Loop replaced by vector ops following tip from johannes.korsawe@volkswagen.de
% 1/19/2006
%
[ncnt,bin]=histc(xi,x);
bin=min(bin,n-1);
bb=bin(1:length(xi));
wj=xi-x(bb);
yi=((wj.*d(bb) +c(bb)).*wj+b(bb)).*wj+y(bb);


function [H,Q,M,W]=matExpIntFam(T,A,B,Qc)
%%MATEXPINTFAM This function explicitly solves a number of real matrix
%       integral problems that arise when solving for optimal linear
%       regulators. The integrals are:
%       H=int_0^T expm(A*s)*B ds
%       Q=int_0^T expm(A'*s)*Qc*expm(A*s) ds
%       M=int_0^T expm(A'*s)*Qc*H(s) ds
%       W=int_0^T H(s)'*Qc*H(s) ds
%       where s is the scalar parameter of integration, expm is the matrix
%       exponential function int refers to a definite integral, and the
%       function
%       H(s) is defined to be
%       H(s)=(int_0^sexpm(A*tau) dtau)*B
%
%INPUTS: T The real, positive, scalar upper bound of the integrals.
%        A A real nXn matrix.
%        B A real nXp matrix. If this parameter is omitted or an empty
%          matrix is passed, then it is assumed that p=0, in which case H,
%          M, and W will be empty matrices.
%       Qc A symmetric positive (semi)definite nXn matrix. If omitted or an
%       empty matrix is passed, then it is assumed that Qc=eye(n,n);
%
%OUTPUTS: H,Q,M,W The values of the desired matrix integrals, which are
%         defined above
%
%Expressions for obtaining the solutions to the integrals by evaluating a
%single matrix exponential are given in [1]. The type of linear regulator
%problem addressed by these integrals is given in [2].
%
%REFERENCES:
%[1] C. F. Van Loan, "Computing Integrals Involving the Matrix
%    Exponential," IEEE Transactions on Automatic Control, vol. AC-23, no.
%    3, pp. 395-404, Jun. 1967.
%[2] E. S. Armstrong and A. K. Caglayan, "An algorithm for the weighting
%    matrices in the sampled-data optimal linear regulator problem,"
%    National Aeronautics and Space Administration, Langley Research
%    Center, Hampton, VA, Tech. Rep. NASA TN D-8372, Dec. 1976.
%
%September 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);

if(nargin<4||isempty(Qc))
    Qc=eye(n,n);
end

if(isempty(B)||nargin<2)
    p=0;
    B=zeros(n,p);
else
    p=size(B,2);
end

C=[-A.',        eye(n,n), zeros(n,n+p);
    zeros(n,n), -A.',     Qc,   zeros(n,p);
    zeros(n,2*n),         A,    B;
    zeros(p,3*n+p)];

CExp=expm(C*T);

F3=CExp((2*n+1):(3*n),(2*n+1):(3*n));
G2=CExp((n+1):(2*n),(2*n+1):(3*n));
G3=CExp((2*n+1):(3*n),(3*n+1):(3*n+p));
H2=CExp((n+1):(2*n),(3*n+1):(3*n+p));
K1=CExp(1:n,(3*n+1):(3*n+p));

H=G3;
Q=F3.'*G2;
M=F3.'*H2;

temp=B.'*F3.'*K1;
W=temp+temp.';

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function geometry_test016 ( )

%*****************************************************************************80
%
%% GEOMETRY_TEST016 tests CIRCLE_IMP_POINTS_2D, POLYGON_AREA_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  center(1:2,1) =  [ 5.0; -2.0 ];
  r = 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEOMETRY_TEST016\n' );
  fprintf ( 1, '  CIRCLE_IMP_POINTS_2D gets points on a circle;\n' );
  fprintf ( 1, '  POLYGON_AREA_2D finds the area of a polygon.\n' );

  circle_imp_print_2d ( r, center, '  The implicit circle:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The area = %f\n', pi * r * r );

  n = 8;

  p = circle_imp_points_2d ( r, center, n );

  r8mat_print ( 2, n, p, '  Sample results:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For any N, the sampled points define a polygon\n' );
  fprintf ( 1, '  whose area approximates the circle area.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  N      Area\n' );
  fprintf ( 1, '\n' );

  for n = 3 : 24

    p = circle_imp_points_2d ( r, center, n );
    result = polygon_area_2d ( n, p );
    fprintf ( 1, '  %6d  %12f\n', n, result );

  end

  return
end


% Surface fit artifact removal
[x,y] = meshgrid(0:.01:1);
z0 = exp(x+y);

znan = z0;
znan(20:50,40:70) = NaN;
znan(30:90,5:10) = NaN;
znan(70:75,40:90) = NaN;

z = inpaint_nans(znan,3);

% Comparison to griddata
k = isnan(znan);
zk = griddata(x(~k),y(~k),z(~k),x(k),y(k));
zg = znan;
zg(k) = zk;

close all
figure
surf(z0)
title 'Original surface'

figure
surf(znan)
title 'Artifacts (large holes) in surface'

figure
surf(zg)
title(['Griddata inpainting (',num2str(sum(isnan(zg(:)))),' NaNs remain)'])

figure
surf(z)
title 'Inpainted surface'

figure
surf(zg-z0)
title 'Griddata error surface'

figure
surf(z-z0)
title 'Inpainting error surface (Note z-axis scale)'


function [ x, seed ] = hyperball01_sample ( m, n, seed )

%*****************************************************************************80
%
%% HYPERBALL01_SAMPLE uniformly samples the unit hyperball.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Russell Cheng,
%    Random Variate Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998, pages 168.
%
%    Reuven Rubinstein,
%    Monte Carlo Optimization, Simulation, and Sensitivity
%    of Queueing Networks,
%    Krieger, 1992,
%    ISBN: 0894647644,
%    LC: QA298.R79.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(M,N), the points.
%
  x = randn ( m, n );
  norm = ones ( 1, m ) * ( x.^2 );
  norm = sqrt ( norm );
  for i = 1 : m
    x(i,1:n) = x(i,1:n) ./ norm(1:n);
  end

  for j = 1 : n
    r = rand ( 1, 1 );
    x(1:m,j) = r ^ ( 1.0 / m ) * x(1:m,j);
  end

  return
end


%% ---------------Truncated Amplitude Spectral Initializer-----------------

% Intializer proposed for Truncated Amplitude Flow as given in Algorithm 1
% of the Truncated Amplitude Flow (TAF) paper. For certain definitions and
% descriptions, user may need to refer to equations (14, 17) and Algorithm
% box 1 for details.
%
% The authors of the paper propose two different initialization schemes
% (this function implements the second approach, described below).
% In the first approach, one throws out the measurements of large
% magnitude, and only keeps the remaining measurement vectors that produce
% small measurements.  These vectors are nearly orthogonal to the signal,
% and so we approximate the signal by finding a vector that is
% un-correlated with these measurement vectors. This requires finding the
% smallest eigenvalue of a large matrix.
%    In the second method, the authors propose to throw out the smallest
% measurements, and find the signal most correlated with the remaining
% measurement vectors (which produce large measurement) by finding the
% largest eigenvalue of a matrix.
%    For certain isometric measurement matrices, these appraoches are both
% equivalent.  However, for non-isometric matrices they differ.  The
% authors of the paper claim that it is difficult to find a small
% eigenvector, and so they adopt the second method that requires a large
% eigenvector.  This second method, which requires the leading (largest)
% eigenvalue/vector, is implemented here.
%   Note:  in practice, small eigenvalues can be computed efficiently using
% an Arnoldi method, and this often leads to better initializers.  For an
% implementation using the smallest eigenvalue/vector, see initNull.m.
%
%  See the script 'testInitOrthogonal.m' for an example of proper usage of
%  this function.

% PAPER TITLE:
%              Solving Systems of Random Quadratic Equations via Truncated
%              Amplitude Flow.

% ARXIV LINK:
%              https://arxiv.org/pdf/1605.08285.pdf

% INPUTS:
%         A:   Function handle/numerical matrix for data matrix A. The rows
%              of this matrix are the measurement vectors that produce
%              amplitude measurements '\psi'.
%         At:  Function handle/numerical matrix for A transpose.
%         b0:  Observed data vector consisting of amplitude measurements
%              generated from b0 = |A*x|. We assign it to 'psi' to be
%              consistent with the notation in the paper.
%         n:   Length of unknown signal to be recovered by phase retrieval.

% OUPTUT :
%         x0:  The initial vector to be used by any solver.

% Note:        When a function handle is used, the value of 'n' (the length
%              of the unknown signal) and 'At' (a function handle for the
%              adjoint of 'A') must be supplied. When 'A' is numeric, the
%              values of 'At' and 'n' are ignored and inferred from the
%              arguments

% DESCRIPTION:
%              Random vectors in high dimensions are almost always
%              orthogonal to each other. Using this very intuitive result,
%              this method computes an initial guess that is orthogonal to
%              as many measurement vectors as possible. The orthogonal
%              promoting initializer solves the limitations of the spectral
%              methods, namely heavy tailed distributions due to 4th moment
%              generating functions by using a truncation step.
%              Specifically, it removes measurement vectors that are not
%              orthogonal to the initial random guess. We form the matrix Y
%              = (1/card_I) * S_bar^T* S_bar using the truncated vectors
%              and compute the leading eigenvector. S_bar is the complement
%              of the set S. Refer to the paper for a description of S.

% METHOD:
%         1.) Find the set 'I_Bar'. I is set of indices of |I| smallest
%             measurement vectors as defined in equation(14) in the paper,
%             where |I| is the cardinality of I.I_bar is the complement of
%             I. This is done because by using I, we are required to find
%             the smallest eignevalue of a matrix formed from the data
%             vectors. However, finding the smallest eigenvalue is often
%             intractible in practice. So we find the largest eigenvalue
%             instead.

%         2.) Using a mask R, form the matrix Y = (1/card_I) * S_bar^T *
%             S_bar where S_bar is the complement of the S which is defined
%             in equation (17) in the Truncated Amplitude Paper.

%         3.) Compute the leading eigenvector of Y (computed in previous
%             step) and scale it according to the norm of x as described in
%             Step 3, Algorithm 1 of the paper.
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

function [x0] = initAmplitude(A,At,b0,n,verbose)

psi = b0; % To be consistent with the notation used in the paper

% If A is a matrix, infer n and At from A
if isnumeric(A)
    n = size(A, 2);
    % Transform matrix into function form
    At = @(x) A' * x;
    A = @(x) A * x;
end

m = length(psi);    % Number of measurements.

if ~exist('verbose','var') || verbose
fprintf(['Estimating signal of length %d using an orthogonal ',...
        'initializer with %d measurements...\n'],n,m);
end

% Cardinality of I. I is the set that contains the indices of the
% truncated vectors. Namely, it removes measurement vectors that are
% not orthogonal to the initial random guess
card_I = ceil(m/6);

% STEP 1: Construct the set I of indices. We approximate by assuming that
% the norm of each row of A is same.
[~, index_array] = sort(psi, 'descend');  % Sort the data vector
ind = index_array(1: card_I); % Truncate the indexes.

% STEP 2: Form Y
R = zeros(m, 1);
% Defining the mask for truncation
R(ind) = 1;
% Forming the truncated matrix Y according to equation (17) in referenced paper.
Y = @(x) 1/card_I * At(R.*A(x));

% STEP 3: Use eigs to compute leading eigenvector of Y (Y is computed in
% previous step)
opts = struct;
opts.isreal = false; % Create opts struct for eigs
[V, ~] = eigs(Y, n, 1, 'lr', opts);
% Scale the norm to match that of x
AV = abs(A(V));
alpha = (AV'*psi) / ( AV'*AV );
x0 = V*alpha;

if ~exist('verbose','var') || verbose
    fprintf('Initialization finished.\n');
end

end


function [shortestPath, totalCost] = dijkstra(netCostMatrix, s, d)
%==============================================================
% shortestPath: the list of nodes in the shortestPath from source to destination;
% totalCost: the total cost of the  shortestPath;
% farthestNode: the farthest node to reach for each node after performing the routing;
% n: the number of nodes in the network;
% s: source node index;
% d: destination node index;
%==============================================================
%  Code by:
% ++by Xiaodong Wang
% ++23 Jul 2004 (Updated 29 Jul 2004)
% ++http://www.mathworks.com/matlabcentral/fileexchange/5550-dijkstra-shortest-path-routing
% Modifications (simplifications) by Meral Shirazipour 9 Dec 2009
%==============================================================
n = size(netCostMatrix,1);
for i = 1:n
    % initialize the farthest node to be itself;
    farthestPrevHop(i) = i; % used to compute the RTS/CTS range;
    farthestNextHop(i) = i;
end

% all the nodes are un-visited;
visited(1:n) = false;

distance(1:n) = inf;    % it stores the shortest distance between each node and the source node;
parent(1:n) = 0;

distance(s) = 0;
for i = 1:(n-1),
    temp = [];
    for h = 1:n,
         if ~visited(h)  % in the tree;
             temp=[temp distance(h)];
         else
             temp=[temp inf];
         end
     end;
     [t, u] = min(temp);      % it starts from node with the shortest distance to the source;
     visited(u) = true;         % mark it as visited;
     for v = 1:n,                % for each neighbors of node u;
         if ( ( netCostMatrix(u, v) + distance(u)) < distance(v) )
             distance(v) = distance(u) + netCostMatrix(u, v);   % update the shortest distance when a shorter shortestPath is found;
             parent(v) = u;     % update its parent;
         end;
     end;
end;

shortestPath = [];
if parent(d) ~= 0   % if there is a shortestPath!
    t = d;
    shortestPath = [d];
    while t ~= s
        p = parent(t);
        shortestPath = [p shortestPath];

        if netCostMatrix(t, farthestPrevHop(t)) < netCostMatrix(t, p)
            farthestPrevHop(t) = p;
        end;
        if netCostMatrix(p, farthestNextHop(p)) < netCostMatrix(p, t)
            farthestNextHop(p) = t;
        end;

        t = p;
    end;
end;

totalCost = distance(d);

%return;


function [F,totalCost,exitCode]=minCostFlow(AMat,CMat,b,maxIter)
%%MINCOSTFLOW Solve the minimum cost flow problem using a strong polynomial
%             time cycle cancelling algorithm that works with non-integer
%             costs. Minimum cost flow problems include transportation
%             problems.
%
%INPUTS: AMat An NXN matrix of costs in a directed graph such that
%             AMat(i,j) is the cost of an edge going from vertex i to
%             vertex j. If no vertex goes from node i to node j, then a
%             cost of 0 should be inserted.
%        CMat An NXN matrix of capacities of the edges in the graph.
%             CMat(i,j) is the capacity of an edge from node i to node j.
%             If no edge exists, then a capacity of zero should be used. It
%             is assumed that all capacities are finite.
%           b An NX1 vector of the supply provided by each node. It is
%             required that sum(b)=0.
%     maxIter An optional parameter specifying the maximum number of
%             iterations that should be performed by the algorithm. If
%             omitted, a maximum of 100+10N iterations is used.
%
%OUTPUTS: F The flow matrix. F(i,j) is the amount of flow going from node i
%           to node j. Note that F(i,j)=-F(j,i). If the algorithm could not
%           obtain an initial feasible solution, then an empty matrix is
%           returned. If the maximum number of iterations is exceeded, then
%           the algorithm will return a feasible solution that is not
%           optimal.
% totalCost The total cost of the flow. This is the quantity being
%           minimized. If the algorithm could not obtain an initial
%           feasible solution, then an empty matrix is returned.
%  exitCode A parameter indicating whether an error occurred or whether the
%           algorithm terminated successfully. Possible values are:
%           0 The algorithm was successful.
%           1 No feasible solution could be found.
%           2 The maximum number of iterations was reached.
%           3 Inputs are invalid. This means that either C contains NaN
%             values, sum(b)~=0 or AMat contains nonfinite values.
%
%The minimum cost flow problems seeks to find a flow F to minimize
%sum(sum(AMat.*F)) subject to the constraints:
%4) Total Vertex Flow: sum(F(i,:))=b(i) for all i.
%2) Capacity Constraint: 0<=F(u,v)<=CMat(u,v)
%3) Skew Symmetry: F(u,v)=-F(v,u)
%Whereas the maximum flow problem, which is solved in the function
%solveMaxFlowEdmondsKarp seeks to maximize the flow between two nodes the
%minimum cost flow problem seeks to minimize the total cost of a fixed
%amount of flow.
%
%The strong polynomial cycle cancelling algorithm of [1] is used. However,
%the epsilon-based convergence acceleration method described in [1] is not
%used. Rather, the algorithm is more similar to the simple cycle
%cancellation algorithm of Chapter 7.2 of [2], except the minimum mean
%cycle is always augmented rather than any negative cost cycle.
%
%The complexity of the non-epsilon accelerated algorithm in [1] is bounded
%as O(N^2*m^3*log(N)), where N is the number of vertices in the graph, and
%m is the number of edges. The true complexity depends on the efficiency of
%the function computeResidualCapacity, which is used to obtain the negative
%cycles.
%
%EXAMPLE: This is example 7.1 in Chapter 7.2 of [2].
% AMat=[0, 4, 1, 0;
%       0, 0, 2, 5;
%       0, 3, 0, 2;
%       0, 0, 0, 0];
% CMat=[0, 2, 2, 0;
%       0, 0, 1, 1;
%       0, 1, 0, 1;
%       0, 0, 0, 0];%Capacity matrix
% b=[2;0;0;-2];
% [F,totalCost,exitCode]=minCostFlow(AMat,CMat,b)
%One will get a flow matrix of
% F =[ 0     0     2     0;
%      0     0    -1     1;
%     -2     1     0     1;
%      0    -1    -1     0];
%having a total cost of 12.
%
%OTHER EXAMPLES: Transportation problems can be transformed into minimum
%cost flow problems and vice versa, as shown in Chapters 7.4 and 7.5 of
%[2]. Thus, the examples in the comments to the function
%solveTransportationProblem can be taken as additional examples of the
%minimum cost flow problem.
%
%REFERENCES:
%[1] A. V. Goldberg and R. E. Tarjan, "Finding minimum-cost circulations
%    by canceling negative cycles," Journal of the Association for
%    Computing Machinery, vol. 36, no. 4, pp. 873-886, Oct. 1989.
%[2] C. H. Papadimitriou and K. Steiglitz, Combinatorial Optimization:
%    Algorithms and Complexity. Englewood Cliffs, NJ: Prentice-Hall Inc.,
%    1982.
%
%July 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numVertex=length(b);

if(nargin<5||isempty(maxIter))
    maxIter=100+10*numVertex;
end

if(any(isnan(CMat(:)))||sum(b)~=0||any(~isfinite(AMat(:))))
    exitCode=3;%The problem is infeasible.
    F=[];
    totalCost=[];
    return;
end

%Step 1: Find a feasible flow.
%In this step, we must find a feasible flow F. This is a flow that
%satisfies constraints 1-3 but does not necessarily minimize the cost
%sum(sum(AMat.*F)). This can be done by ignoring costs and solving a
%maximum flow problem based only on the constraints. The maximum flow
%problem does not have demand constraints on all of the nodes, rather it
%only has them on a sink and source which are added.
%
%To enforce constraint 1, we have to create a network with an artificial
%sink and source. If b(i) is positive, then we need an arc from the source
%to that node with capacity b(i). If b(i) is negative, then we need an arc
%from the sink to that node with capacity -b(i). If the maximum flow
%problem solved on the modified graph saturates all of the edges coming out
%of the source, then the problem is feasible.

%Augment the capacity matrix.
CMatAugment=[CMat,zeros(numVertex,2);
             zeros(2,numVertex+2)];

source=numVertex+1;
sink=numVertex+2;
totalFlowIn=0;
for curVertex=1:numVertex
    if(b(curVertex)>0)
        CMatAugment(source,curVertex)=b(curVertex);
        totalFlowIn=totalFlowIn+b(curVertex);
    else
        CMatAugment(curVertex,sink)=-b(curVertex);
    end
end

[maxFlow,F]=solveMaxFlowEdmondsKarp(CMatAugment,source,sink);

%Check whether all of the source nodes have been saturated. This is true if
%the total flow in equals the maximum flow found. A comparison to an
%epsilon value is used as it is not clear whether it can be guaranteed
%within finite precision bounds that the two quantities will always be
%equal for feasible problems, though they seem to generally be equal for
%feasible problems.
if(abs(maxFlow-totalFlowIn)>eps(totalFlowIn))
    exitCode=1;%The problem is infeasible.
    F=[];
    totalCost=[];
    return;
end

%Get rid of the artificial source and sink nodes from the flow matrix.
F=F(1:numVertex,1:numVertex);

%To make the computation of the residual matrix simpler, we will transform
%the cost matrix. This simplifies the computation of costs in the residual
%flow network.
AMatOrig=AMat;
AMat=AMat-AMat';

%Compute the residual flow network, also known as the incremental flow
%network. Definition 7.3 from Chapter 7.2 of [2] explains what the
%incremental flow network is and how the costs are assigned. The residual
%capacity is updated during augmentation of the flow matrix F, so that it
%need not be recomputed each loop.

%Find the capacity of the residual flow network.
residualCapacity=CMat-F;
for curIter=1:maxIter
    %Step 2: Find the minimum mean cost cycle with respect to the costs on
    %        the residual flow network, also known as the incremental flow
    %        network.

    %The costs associated with the residual flow network are AMat(i,j) on
    %forward nodes and -AMat(i,j) on backward nodes. Hence the reason why
    %we replaced AMat with AMat-AMat'.
    costMat=Inf(numVertex,numVertex);
    costMat(residualCapacity~=0)=AMat(residualCapacity~=0);

    [minCycleMean,cycleVertices]=findMinMaxCycleMean(costMat,true);

    if(minCycleMean>=0)
        exitCode=0;
        FP=F;
        FP(F<0)=0;
        totalCost=sum(sum(FP.*AMatOrig));
        return;
    end

    %Step 3: The minimum mean cycle is negative. We must cancel the minimum
    %        mean cycle. That is, we augment the graph along the negative
    %        cycle. We adjust the flow around the cycle by as much
    %        as possible without violating capacity constraints, so that
    %        the negative cycle no longer exists.

    %Determine the minimum residual capacity of the cycle.
    numVertexInCycle=length(cycleVertices);
    minCapacity=Inf;
    startVertex=cycleVertices(1);
    for curEndVertex=2:numVertexInCycle
        endVertex=cycleVertices(curEndVertex);

        curCapacity=residualCapacity(startVertex,endVertex);
        minCapacity=min(curCapacity,minCapacity);

        startVertex=endVertex;
    end

    %Push the amount of flow around the negative cycle that will saturate
    %the minimum capacity arc on the minimum mean cycle. Note that the skew
    %symmetry constraint dictates how the flow is to be added (how
    %augmentation is performed).
    startVertex=cycleVertices(1);
    for curEndVertex=2:numVertexInCycle
        endVertex=cycleVertices(curEndVertex);

        %Update the flow
        F(startVertex,endVertex)=F(startVertex,endVertex)+minCapacity;
        F(endVertex,startVertex)=F(endVertex,startVertex)-minCapacity;

        %Update the residual flow network. The update is opposite that of
        %F, because the residual flow is CMat-F.
        residualCapacity(startVertex,endVertex)=residualCapacity(startVertex,endVertex)-minCapacity;
        residualCapacity(endVertex,startVertex)=residualCapacity(endVertex,startVertex)+minCapacity;

        startVertex=endVertex;
    end
end

%Find the cost of the flow.
FP=F;
FP(F<0)=0;
totalCost=sum(sum(FP.*AMatOrig));

%If we are here, the maximum number of iterations has elapsed. We will
%check whether convergence occurred on the final iteration, and it not,
%then we will return accordingly.
costMat=Inf(numVertex,numVertex);
costMat(residualCapacity~=0)=AMat(residualCapacity~=0);

minCycleMean=findMinMaxCycleMean(costMat,true);
if(minCycleMean>=0)
    exitCode=0;
    return;
end

%If we get here, then the algorithm terminated without cancelling all
%negative cycles. Thus, the algorithm did not converge.
exitCode=2;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [xi,w]=fifthOrderPyramidCubPoints()
%%FIFTHORDERPYRAMIDCUBPOINTS Generate fifth-order cubature points for
%  integration over a 3-dimensional pyramid with a square base with the
%  peak vertex at (0,0,1) and the base vertices at (1,-1,-1), (-1,-1,-1),
%  (-1,1,-1), and (1,1,-1).
%
%INPUTS: None
%
%OUTPUTS: xi This is a 3XnumCubPoints set of points for the standard
%            square pyramid.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard square pyramid (8/3).
%
%This function implements the points given in [1] (15 points).
%
%EXAMPLE:
%We compare a 5th-order moment computed using these cubature points
%to one computed using monomialIntPyramid. The results are the same within
%typical finite precision limits.
% [xi,w]=fifthOrderPyramidCubPoints();
% alpha=[2;2;1];
% theMoment=findMomentFromSamp(alpha,xi,w);
% intVal=monomialIntPyramid(alpha);
% RelErr=(theMoment-intVal)/intVal
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[                                     0,                                          0,   0.45971576156501338586164265377920811314,   0.18249431975770692138374895897213800931;
                                        0,                                          0,  -0.39919795837246198593385139590712322914,   0.45172563864726406056400285032640105704;
                                        0,                                          0,  -0.99999998701645569241460017355590234925,   0.15654542887619877154120304977336547704;
 0.70652603154632457420722562974792066862,                                          0,                                      -0.75,   0.20384344839498724639142514342645843799;
                                        0,   0.70652603154632457420722562974792066862,                                      -0.75,   0.20384344839498724639142514342645843799;
-0.70652603154632457420722562974792066862,                                          0,                                      -0.75,   0.20384344839498724639142514342645843799;
                                        0,  -0.70652603154632457420722562974792066862,                                      -0.75,   0.20384344839498724639142514342645843799;
 0.70511712277882760181079385797948261057,   0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
 0.70511712277882760181079385797948261057,  -0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
-0.70511712277882760181079385797948261057,   0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
-0.70511712277882760181079385797948261057,  -0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
 0.43288286410354097685000790909815143591,   0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173;
 0.43288286410354097685000790909815143591,  -0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173;
-0.43288286410354097685000790909815143591,   0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173;
-0.43288286410354097685000790909815143591,  -0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173];

w=M(:,4);
xi=M(:,1:3)';

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function V=covnw(data,nlag,demean)
% Long-run covariance estimation using Newey-West (Bartlett) weights
%
% USAGE:
%   [V] = covnw(DATA)
%   [V] = covnw(DATA,NLAG,DEMEAN)
%
% INPUTS:
%   DATA   - T by K vector of dependent data
%   NLAG   - Non-negative integer containing the lag length to use.  If empty or not included,
%              NLAG=min(floor(1.2*T^(1/3)),T) is used
%   DEMEAN - Logical true or false (0 or 1) indicating whether the mean should be subtracted when
%              computing the covariance
%
% OUTPUTS:
%   V      - A K by K covariance matrix estimated using Newey-West (Bartlett) weights
%
% COMMENTS:
%
% EXAMPLES:
%   Simulate an AR(1)
%       y = armaxfilter_simulate(1000,0,1,.9);
%   Newey-West covariance with automatic BW selection
%       lrcov = covnw(y)
%   Newey-West covariance with 10 lags
%       lrcov = covnw(y, 10)
%   Newey-West covariance with 10 lags and no demeaning
%       lrcov = covnw(y, 10, 0)
%
% See also COVVAR

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 5/1/2007

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T=size(data,1);
if nargin==1
    nlag=min(floor(1.2*T^(1/3)),T);
    demean=true;
elseif nargin==2
    demean=true;
end
if isempty(nlag)
    nlag=min(floor(1.2*T^(1/3)),T);
end
if isempty(demean)
    demean=true;
end
if ~ismember(demean,[0 1])
    error('DEMEAN must be either logical true or false.')
end
if floor(nlag)~=nlag || nlag<0
    error('NLAG must be a non-negative integer.')
end
if ndims(data)>2
    error('DATA must be a T by K matrix of data.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if demean
    data=data-repmat(mean(data),T,1);
end

% NW weights
w=(nlag+1-(0:nlag))./(nlag+1);
% Start the covariance
V=data'*data/T;
for i=1:nlag
    Gammai=(data((i+1):T,:)'*data(1:T-i,:))/T;
    GplusGprime=Gammai+Gammai';
    V=V+w(i+1)*GplusGprime;
end


%% Optimization of a simple (Rosenbrock) function, with no constraints
% The unconstrained solution is at [1,1]
rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;

% With no constraints, operation simply passes through
% directly to fminsearch. The solution should be [1 1]
xsol = fminsearchbnd(rosen,[3 3])

%% Full lower and upper bound constraints which will all be inactive
xsol = fminsearchbnd(rosen,[3 3],[-1 -1],[4 4])

%% Only lower bound constraints
xsol = fminsearchbnd(rosen,[3 3],[2 2])

%% Only upper bound constraints
xsol = fminsearchbnd(rosen,[-5 -5],[],[0 0])

%% Dual constraints
xsol = fminsearchbnd(rosen,[2.5 2.5],[2 2],[3 3])

%% Dual constraints, with an infeasible starting guess
xsol = fminsearchbnd(rosen,[0 0],[2 2],[3 3])

%% Mixed constraints
xsol = fminsearchbnd(rosen,[0 0],[2 -inf],[inf 3])

%% Provide your own fminsearch options
opts = optimset('fminsearch');
opts.Display = 'iter';
opts.TolX = 1.e-12;

n = [10,5];
H = randn(n);
H=H'*H;
Quadraticfun = @(x) x*H*x';

% Global minimizer is at [0 0 0 0 0].
% Set all lower bound constraints, all of which will
% be active in this test.
LB = [.5 .5 .5 .5 .5];
xsol = fminsearchbnd(Quadraticfun,[1 2 3 4 5],LB,[],opts)

%% Exactly fix one variable, constrain some others, and set a tolerance
opts = optimset('fminsearch');
opts.TolFun = 1.e-12;

LB = [-inf 2 1 -10];
UB = [ inf  inf 1  inf];
xsol = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB,opts)

%% All the standard outputs from fminsearch are still returned
[xsol,fval,exitflag,output] = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB)


function d=v_distchpf(pf1,pf2,mode)
%V_DISTCHPF calculates the cosh spectral distance between power spectra D=(PF1,PF2,MODE)
%
% Inputs: PF1,PF2     Power spectra to be compared. Each row represents a power spectrum: the first
%                     and last columns represent the DC and Nyquist terms respectively.
%                     PF1 and PF2 must have the same number of columns.
%
%         MODE        Character string selecting the following options:
%                         'x'  Calculate the full distance matrix from every row of PF1 to every row of PF2
%                         'd'  Calculate only the distance between corresponding rows of PF1 and PF2
%                              The default is 'd' if PF1 and PF2 have the same number of rows otherwise 'x'.
%
% Output: D           If MODE='d' then D is a column vector with the same number of rows as the shorter of PF1 and PF2.
%                     If MODE='x' then D is a matrix with the same number of rows as PF1 and the same number of columns as PF2'.
%
% The COSH spectral distance is the average over +ve and -ve frequency of
%
%                     cosh(log(p1/p2))-1   =   (p1-p2)^2/(2p1*p2)   =   (p1/p2 + p2/p1)/2 - 1
%
% The COSH distance is a symmetrical version of the Itakura-Saito distance: v_distchpf(x,y)=(v_distispf(x,y)+v_distispf(y,x))/2

% The Cosh distance can also be calculated directly from AR coefficients; providing np is large
% enough, the values of d0 and d1 in the following will be very similar:
%
%         np=255; d0=v_distchar(ar1,ar2); d1=v_distchpf(v_lpcar2pf(ar1,np),v_lpcar2pf(ar2,np))
%

% Ref: A.H.Gray Jr and J.D.Markel, "Distance measures for speech processing", IEEE ASSP-24(5): 380-391, Oct 1976
%      L. Rabiner abd B-H Juang, "Fundamentals of Speech Recognition", Section 4.5, Prentice-Hall 1993, ISBN 0-13-015157-2

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_distchpf.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 2 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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf1,p2]=size(pf1);
p1=p2-1;
nf2=size(pf2,1);
if nargin<3 | isempty(mode) mode='0'; end
if any(mode=='d') | (mode~='x' & nf1==nf2)
   nx=min(nf1,nf2);
   r=pf1(1:nx,:)./pf2(1:nx,:);
   q=r+r.^(-1);
   d=(2*sum(q(:,2:p1),2)+q(:,1)+q(:,p2))/(4*p1)-1;
else
   r=permute(pf1(:,:,ones(1,nf2)),[1 3 2])./permute(pf2(:,:,ones(1,nf1)),[3 1 2]);
   q=r+r.^(-1);
   d=(2*sum(q(:,:,2:p1),3)+q(:,:,1)+q(:,:,p2))/(4*p1)-1;
end


function h = p37_h ( n, x )

%*****************************************************************************80
%
%% P37_H evaluates the Hessian for problem 37.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  arg = - ( x(1) - pi )^2 - ( x(2) - pi )^2;
  dargdx1 = - 2.0 * ( x(1) - pi );
  dargdx2 = - 2.0 * ( x(2) - pi );

  factor = cos ( x(2) ) * ( sin ( x(1) ) - cos ( x(1) ) * dargdx1 );
  dfdx1 = cos ( x(2) ) * ( cos ( x(1) ) + sin ( x(1) ) * dargdx1 + 2.0 * cos ( x(1) ) );
  dfdx2 = - sin ( x(2) ) * ( sin ( x(1) ) - cos ( x(1) ) * dargdx1 );

  h(1,1) = ( dfdx1 + factor * dargdx1 ) * exp ( arg );
  h(1,2) = ( dfdx2 + factor * dargdx2 ) * exp ( arg );

  factor = cos ( x(1) ) * ( sin ( x(2) ) - cos ( x(2) ) * dargdx2 );
  dfdx1 = - sin ( x(1) ) * ( sin ( x(2) ) - cos ( x(2) ) * dargdx2 );
  dfdx2 = cos ( x(1) ) * ( cos ( x(2) ) + sin ( x(2) ) * dargdx2 ...
    + 2.0 * cos ( x(2) ) );

  h(2,1) = ( dfdx1 + factor * dargdx1 ) * exp ( arg );
  h(2,2) = ( dfdx2 + factor * dargdx2 ) * exp ( arg );

  return
end


function value = p19_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P19_F evaluates the integrand for problem 19.
%
%  Dimension:
%
%    DIM_NUM is arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    Z defaults to (1/3,1/3,...,1/3).
%    You can reset Z by calling P19_R8VEC.
%
%  Integrand:
%
%    f(x) = product ( sqrt ( abs ( x(1:dim_num) - z(1:dim_num) ) ) )
%
%  Exact Integral:
%
%    With Z as given,
%
%      (2/3)**DIM_NUM * ( (2/3)**(3/2) + (1/3)**(3/2) )**DIM_NUM
%
%    or approximately 0.49**DIM_NUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arnold Krommer, Christoph Ueberhuber,
%    Numerical Integration on Advanced Systems,
%    Springer, 1994,
%    ISBN: 3540584102,
%    LC: QA299.3.K76.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  z = [];
  z = p19_r8vec ( 'G', 'Z', dim_num, z );

  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = prod ( sqrt ( abs ( x(1:dim_num,point) - z(1:dim_num)' ) ) );
  end

  p19_i4 ( 'I', '#', point_num );

  return
end


function y = contract(x,i,j)
%CONTRACT Contract tensor along two dimensions (array trace).
%
%   Y = CONTRACT(X,I,J) contracts the entries of X along dimensions I
%   and J. Contraction is a generalization of matrix trace. In other
%   words, the trace is performed along the two-dimensional slices
%   defined by dimensions I and J. It is possible to implement tensor
%   multiplication as an outer product followed by a contraction.
%
%   Examples
%   X = tensor(rand(4,3,2)); Y = tensor(rand(3,2,4));
%   Z1 = ttt(X,Y,1,3); %<-- Normal tensor multiplication
%   Z2 = contract(ttt(X,Y),1,6); %<-- Outer product + contract
%   norm(Z1-Z2) %<-- Should be zero
%
%   See also TENSOR, TENSOR/TTT.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% Error checking
if x.size(i) ~= x.size(j)
    error('Must contract along equally sized dimensions');
end

% Error checking
if i == j
    error('Must contract along two different dimensions');
end

% Easy case - returns a scalar
if ndims(x) == 2
    y = trace(x.data);
    return;
end

% Remaining dimensions after trace
remdims = setdiff(1:ndims(x),[i j]);

% Size for y
newsize = x.size(remdims);

% Total size of remainder
m =  prod(newsize);

% Number of items to add for trace
n = x.size(i);

% Permute trace dimensions to the end
x = permute(x, [remdims i j]);

% Reshape data to be 3D
data = reshape(x.data, m, n, n);

% Add diagonal entries for each slice
newdata = zeros(m,1);
for i = 1:n
    newdata = newdata + data(:,i,i);
end

% Reshape result
if numel(newsize) > 1
    newdata = reshape(newdata,newsize);
end
y = tensor(newdata,newsize);


function value = p07_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P07_F evaluates the integrand for problem 07.
%
%  Dimension:
%
%    N arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integrand:
%
%    product ( pi / 2 ) * sin ( pi * x(1:dim_num) )
%
%  Exact Integral:
%
%    1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = prod ( 0.5 * pi * sin ( pi * x(1:dim_num,point) ) );
  end

  p07_i4 ( 'I', '#', point_num );

  return
end


function [Q, R] = gs_c(A)
%GS_C    Classical Gram-Schmidt QR factorization.
%        [Q, R] = GS_C(A) uses the classical Gram-Schmidt method to compute the
%        factorization A = Q*R for m-by-n A of full rank,
%        where Q is m-by-n with orthonormal columns and R is n-by-n.

%        Reference:
%        N. J. Higham, Accuracy and Stability of Numerical Algorithms,
%        Second edition, Society for Industrial and Applied Mathematics,
%        Philadelphia, PA, 2002; sec 19.8.

[m, n] = size(A);
Q = zeros(m,n);
R = zeros(n);

for j=1:n
    R(1:j-1,j) = Q(:,1:j-1)'*A(:,j);
    temp = A(:,j) - Q(:,1:j-1)*R(1:j-1,j);
    R(j,j) = norm(temp);
    Q(:,j) = temp/R(j,j);
end


% EX_LAPLACE_SQUARE: solve the Poisson problem in the unit square with a B-spline discretization.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';

% Type of boundary conditions for each side of the domain
problem_data.nmnn_sides   = [];
problem_data.drchlt_sides = [1 2 3 4];

% Physical parameters
problem_data.c_diff  = @(x, y) ones(size(x));

% Source and boundary terms
problem_data.f = @(x, y) zeros (size (x));
problem_data.g = @test_square_g_nmnn;
problem_data.h = @(x, y, ind) exp (x) .* sin(y);

% Exact solution (optional)
problem_data.uex     = @(x, y) exp (x) .* sin (y);
problem_data.graduex = @(x, y) cat (1, ...
                       reshape (exp(x).*sin(y), [1, size(x)]), ...
                       reshape (exp(x).*cos(y), [1, size(x)]));

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [3 3];       % Degree of the splines
method_data.regularity = [2 2];       % Regularity of the splines
method_data.nsub       = [9 9];       % Number of subdivisions
method_data.nquad      = [4 4];       % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER

[geometry, msh, space, u] = solve_laplace (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) EXPORT TO PARAVIEW

output_file = 'Square_BSP_Deg3_Reg2_Sub9';

vtk_pts = {linspace(0, 1, 20), linspace(0, 1, 20)};
fprintf ('The result is saved in the file %s \n \n', output_file);
sp_to_vtk (u, space, geometry, vtk_pts, output_file, 'u')

% 4.2) PLOT IN MATLAB. COMPARISON WITH THE EXACT SOLUTION

[eu, F] = sp_eval (u, space, geometry, vtk_pts);
[X, Y]  = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));
subplot (1,2,1)
surf (X, Y, eu)
title ('Numerical solution'), axis tight
subplot (1,2,2)
surf (X, Y, problem_data.uex (X,Y))
title ('Exact solution'), axis tight

% Display errors of the computed solution in the L2 and H1 norm
[error_h1, error_l2] = ...
           sp_h1_error (space, msh, u, problem_data.uex, problem_data.graduex)

%!demo
%! ex_laplace_square

%!test
%! problem_data.geo_name = 'geo_square.txt';
%! problem_data.nmnn_sides   = [];
%! problem_data.drchlt_sides = [1 2 3 4];
%! problem_data.c_diff  = @(x, y) ones(size(x));
%! problem_data.f = @(x, y) zeros (size (x));
%! problem_data.g = @test_square_g_nmnn;
%! problem_data.h = @(x, y, ind) exp (x) .* sin(y);
%! problem_data.uex     = @(x, y) exp (x) .* sin (y);
%! problem_data.graduex = @(x, y) cat (1, ...
%!                       reshape (exp(x).*sin(y), [1, size(x)]), ...
%!                       reshape (exp(x).*cos(y), [1, size(x)]));
%! method_data.degree     = [3 3];       % Degree of the splines
%! method_data.regularity = [2 2];       % Regularity of the splines
%! method_data.nsub       = [9 9];       % Number of subdivisions
%! method_data.nquad      = [4 4];       % Points for the Gaussian quadrature rule
%! [geometry, msh, space, u] = solve_laplace (problem_data, method_data);
%! [error_h1, error_l2] = ...
%!           sp_h1_error (space, msh, u, problem_data.uex, problem_data.graduex);
%! assert (msh.nel, 81)
%! assert (space.ndof, 144)
%! assert (error_h1, 9.86428525677199e-06, 1e-14)
%! assert (error_l2, 1.68004134750130e-07, 1e-14)


function [q, M] = conversionMatrix2Quaternion(M, varargin)
% conversionMatrix2Quaternion - function to convert Rotation Matrix to Quaternion coeff
%
% Syntax:  q = conversionMatrix2Quaternion(M(3,3))
%          q = conversionMatrix2Quaternion(M(1,1),M(1,2),M(1,3),M(2,1),M(2,2),M(2,3),M(3,1),M(3,2),M(3,3))
%          q = conversionMatrix2Quaternion([theta, psi, phi])
%          q = conversionMatrix2Quaternion(theta, psi, phi)
%
% Inputs:
%          M - Rotation Matrix (3,3)
%     *theta - X rotation angle (deg)
%       *psi - Y rotation angle (deg)
%       *phi - Z rotation angle (deg)
%
% Outputs:
%     q - quaternion values [q0 qx qy qz]
%     M - Rotation Matrix used to compute q
%
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: none;

% Author: Marco Borges, Ph.D. Student, Computer/Biomedical Engineer
% UFMG, PPGEE, Neurodinamica Lab, Brazil
% email address: marcoafborges@gmail.com
% Website: http://www.cpdee.ufmg.br/
% Reference: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
% April 2013; v2; Last revision: 2013-04-18
% Changelog: v2 - add linear velocity extraction

%------------- BEGIN CODE --------------
% M = [m00 m01 m02;
%     m10 m11 m12;
%     m20 m21 m22];

if length(M) == 1 && nargin == 8
    M = [M, varargin{1}, varargin{2}; varargin{3}, varargin{4}, varargin{5}; varargin{6}, varargin{7}, varargin{8}];

elseif length(M) == 9
    M = [M(1), M(2), M(3); M(4), M(5), M(6); M(7), M(8), M(9)];

elseif length(M) == 3 % [theta (x rot), psi (y rot), phi (z rot)]
    theta = M(1); psi = M(2); phi = M(3);
    M = [cos(deg2rad(psi))*cos(deg2rad(phi)), cos(deg2rad(psi))*sin(deg2rad(phi)), -sin(deg2rad(psi));
         (-cos(deg2rad(theta))*sin(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (cos(deg2rad(theta))*cos(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), sin(deg2rad(theta))*cos(deg2rad(psi));
         (sin(deg2rad(theta))*sin(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (-sin(deg2rad(theta))*cos(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), cos(deg2rad(theta))*cos(deg2rad(psi))];

elseif length(M) == 1 && nargin == 3 % (theta (x rot), psi (y rot), phi (z rot))
    theta = M; psi = varargin{1}; phi = varargin{2};
    M = [cos(deg2rad(psi))*cos(deg2rad(phi)), cos(deg2rad(psi))*sin(deg2rad(phi)), -sin(deg2rad(psi));
         (-cos(deg2rad(theta))*sin(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (cos(deg2rad(theta))*cos(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), sin(deg2rad(theta))*cos(deg2rad(psi));
         (sin(deg2rad(theta))*sin(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (-sin(deg2rad(theta))*cos(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), cos(deg2rad(theta))*cos(deg2rad(psi))];
end

tr = M(1,1) + M(2,2) + M(3,3);

if (tr > 0)
    S = sqrt(tr+1.0) * 2; % S=4*qw
    qw = 0.25 * S;
    qx = (M(3,2) - M(2,3)) / S;
    qy = (M(1,3) - M(3,1)) / S;
    qz = (M(2,1) - M(1,2)) / S;
elseif ((M(1,1) > M(2,2)) && (M(1,1) > M(3,3)))
    S = sqrt(1.0 + M(1,1) - M(2,2) - M(3,3)) * 2; % S=4*qx
    qw = (M(3,2) - M(2,3)) / S;
    qx = 0.25 * S;
    qy = (M(1,2) + M(2,1)) / S;
    qz = (M(1,3) + M(3,1)) / S;
elseif (M(2,2) > M(3,3))
    S = sqrt(1.0 + M(2,2) - M(1,1) - M(3,3)) * 2; % S=4*qy
    qw = (M(1,3) - M(3,1)) / S;
    qx = (M(1,2) + M(2,1)) / S;
    qy = 0.25 * S;
    qz = (M(2,3) + M(3,2)) / S;
else
    S = sqrt(1.0 + M(3,3) - M(1,1) - M(2,2)) * 2; % S=4*qz
    qw = (M(2,1) - M(1,2)) / S;
    qx = (M(1,3) + M(3,1)) / S;
    qy = (M(2,3) + M(3,2)) / S;
    qz = 0.25 * S;
end

q = [qw qx qy qz];
%-------------- END CODE ---------------


function theta = polygon3dNormalAngle(points, ind)
%POLYGON3DNORMALANGLE Normal angle at a vertex of the 3D polygon
%
%   THETA = polygon3DNormalAngle(POLYGON, IND)
%   where POLYGON is a set of points, and IND is index of a point in
%   polygon. The function compute the angle of the normal cone localized at
%   this vertex.
%   If IND is a vector of indices, normal angle is computed for each vertex
%   specified by IND.
%
%   Example
%   % create an equilateral triangle in space
%   poly3d = [1 1 0;-1 0 1;0 -1 -1];
%   % compute each normal angle
%   theta = polygon3dNormalAngle(poly3d, 1:size(poly3d, 1));
%   % sum of normal angles must be equal to 2*PI for simple polygons
%   sum(theta)
%
%   IMPORTANT NOTE: works only for convex angles ! ! ! !
%
%   See also
%   polygons3d, faceNormalAngle
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2005-11-30
% Copyright 2005 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

% number of points
np = size(points, 1);

% number of angles to compute
nv = length(ind);

theta = zeros(nv, 1);

for i=1:nv
    p0 = points(ind(i), :);

    if ind(i)==1
        p1 = points(np, :);
    else
        p1 = points(ind(i)-1, :);
    end

    if ind(i)==np
        p2 = points(1, :);
    else
        p2 = points(ind(i)+1, :);
    end

    theta(i) = pi - anglePoints3d(p1, p0, p2);
end


function A=polyarea_signed(V)

% function A=polyarea_signed(V)
%-------------------------------------------------------------------------
%
%
%
% Background (https://demonstrations.wolfram.com/SignedAreaOfAPolygon/):
% The formula for the area of a simple polygon can be elegantly derived
% using Green's theorem and extended to moments of the region. S. F.
% Bockman, "Generalizing the Formula for Areas of Polygons to Moments,"
% Amer. Math. Monthly, 96(2), 1989 pp. 131-132.
%
%-------------------------------------------------------------------------
%%

E=[(1:size(V,1))' [(2:size(V,1))'; 1]]; %Edge array
X=V(:,1); %X coordinates
Y=V(:,2); %Y coordinates
XE=X(E);  %X coordinates of edge points
YE=Y(E); %Y coordinates of edge points
A=sum(0.5*((XE(:,1).*YE(:,2))-(XE(:,2).*YE(:,1)))); %Signed area

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


function H = circlePlane3D( center, normal, radious, theintv, normalon, color, style )
%CIRCLEPLANE3D Summary of this function goes here
%--------------------------------------------------------------------------
%Generate a circle plane in 3D with the given center and radious
%The plane is defined by the normal vector
%theintv is the interval theta which allow you to control your polygon
%shape
% Example:,
%
%   circlePlane3D([0 0 0], [1 -1 2], 5, 0.2, 1, [0 0 1], '-');
%   circlePlane3D([3 3 -3],[0 1 1], 3, 0.1, 1, 'y', '-');
%
%   Cheng-Yuan Wu <ieda_wind@hotmail.com>
%   Version 1.00
%   Aug, 2012
%--------------------------------------------------------------------------
%generate circle polygon
t = 0:theintv:2*pi;
x = radious*cos(t);
y = radious*sin(t);
z = zeros(size(x));
%compute rotate theta and axis
zaxis = [0 0 1];
normal = normal/norm(normal);
ang = acos(dot(zaxis,normal));
axis = cross(zaxis, normal)/norm(cross(zaxis, normal));
% A skew symmetric representation of the normalized axis
axis_skewed = [ 0 -axis(3) axis(2) ; axis(3) 0 -axis(1) ; -axis(2) axis(1) 0];
% Rodrigues formula for the rotation matrix
R = eye(3) + sin(ang)*axis_skewed + (1-cos(ang))*axis_skewed*axis_skewed;
fx = R(1,1)*x + R(1,2)*y + R(1,3)*z;
fy = R(2,1)*x + R(2,2)*y + R(2,3)*z;
fz = R(3,1)*x + R(3,2)*y + R(3,3)*z;
%translate center
fx = fx+center(1);
fy = fy+center(2);
fz = fz+center(3);
H = fill3(fx, fy, fz, color);
if normalon == 1
    hold on;
    H = plot3([center(1) center(1)+normal(1)],[center(2) center(2)+normal(2)],[center(3) center(3)+normal(3)],style);
end

end


%MAIN_simpleHarmonicOscillator.m
%
% This script runs a simulation of a simple harmonic oscillator

% Use: EoM_Single_Pendulum to write the equations of motion

m = 1.0;  % (kg)  mass
k = 1.0;  % (N/m) spring constant

tSpan = [0,10]; %Simulation time interval

x0 = 0.5;  % (m) initial position
v0 = 0;  % (m/s) initial velocit
z0 = [x0;v0];

userFunc = @(t,z)simpleHarmonicOscillatorDynamics(t,z,m,k);

options = odeset(...
    'AbsTol',1e-8,...
    'RelTol',1e-8,...
    'Vectorized','on');

% Run the simulation!
sol = ode45(userFunc,tSpan,z0,options);

% Break apart solution for plotting
nPlot = 1000;
time = linspace(tSpan(1),tSpan(2),nPlot);
z = deval(sol,time); %Evaluate solution from ode45 at points in time
x = z(1,:);
v = z(2,:);

% Plotting!

figure(111);clf;

subplot(2,1,1);
plot(time,x,'k-','LineWidth',2);
xlabel('time (s)')
ylabel('position (m)');

subplot(2,1,2);
plot(time,v,'k-','LineWidth',2);
xlabel('time (s)')
ylabel('velocity (m/s)');


function x = r8mat_lt_solve ( n, a, b )

%*****************************************************************************80
%
%% R8MAT_LT_SOLVE solves a transposed lower triangular linear system.
%
%  Discussion:
%
%    Given the lower triangular matrix A, the linear system to be solved is:
%
%      A' * x = b
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows and columns of the matrix A.
%
%    Input, real A(N,N), the N by N lower triangular matrix.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Output, real X(N), the solution of the linear system.
%
  x(1:n) = 0.0;
%
%  Solve L'*x = b.
%
  for i = n : -1 : 1
    x(i) = ( b(i) - x(i+1:n) * a(i+1:n,i) ) / a(i,i);
  end

  return
end


function Trans = PUMA_TransMatrices(alpha, a, d, theta)
% This creates the Robot Manipulator Link(s) Transformation Matrix.
% The entries of the DH Table are passed in as the parameter.

%% Handle errors (if any) for the parameters passed into the function

% Check for the dimensions of the parameters passed
[~, c(1)] = size(alpha);
[~, c(2)] = size(a);
[~, c(3)] = size(d);
[~, c(4)] = size(theta);

if (c(1) ~= c(2) || c(1) ~= c(3) || c(1) ~= c(4))
    error('Invalid arguments. Size of all the arguments should be same')
    return
end

%% Generate the Transformation Matrices from the DH-parameters passed

% Find the Transformation matrix from the first row of DH-Table
% This will give us the position and orientation of the end of first link
% of the Robot Manipulator
Trans(:,:,1) = PUMA_Transformation(alpha(1), a(1), d(1), theta(1));

% Find the Transformation matrix from the rest of the rows of DH-Table
% This will give us the position and orientation of the rest of the links
% of the Robot Manipulator
for i = 2:c(1)
    Trans(:,:,i)= PUMA_Transformation(alpha(i), a(i), d(i), theta(i), Trans(:,:, i-1));
end

end