function [xi,w]=thirdOrder3DCubPoints()
%%THIRDORDER3DCUBPOINTS Generate third-order cubature points
%               for integration over a 3-dimensional cube with vertices of
%               (1,-1,-1), (-1,-1,-1), (-1,1,-1), (1,1,-1), (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
%            cube.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard cube (8).
%
%This function implements the points given in [1] (6 points).
%
%EXAMPLE:
%We compare a 2nd-order moment computed using these cubature points
%to one computed using monomialIntCube (a 3rd order moment would have
%just been 0). The results are the same within typical finite precision
%limits.
% [xi,w]=thirdOrder3DCubPoints();
% alpha=[2;0;0];
% theMoment=findMomentFromSamp(alpha,xi,w);
% intVal=monomialIntCube(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=[-1,  0,  0,  1.3333333333333333333333333333333333333;
    0,  0,  1,  1.3333333333333333333333333333333333333;
    0,  1,  0,  1.3333333333333333333333333333333333333;
    0,  0, -1,  1.3333333333333333333333333333333333333;
    1,  0,  0,  1.3333333333333333333333333333333333333;
    0, -1,  0,  1.3333333333333333333333333333333333333];

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 [a,f,B,W]=ldatrace(b,w,n,c)
%LDATRACE Calculates an LDA transform to maximize trace discriminant [a,f,B,W]=(b,w,n,c)
% If a feature vector X can come from one of several class and W and B are respectively
% the within-class and between-class covariance matrices, then the generalized Fisher discriminant
% F=trace(W\B) is a measure of how well the feature vector discriminates between the classes.
% If we choose a rectangular (tall, skinny) transformation matrix, we can define a smaller
% feature vector Y=A'*X. The aim of this routine is to choose A to maximize the Fisher
% discriminant. We assume that W is positive definite and B is positive semi-definite.
% The input argument C allows the uset to pre-specify some of the columns of A.
%
% Inputs:
%     w[m,m] = within class covariance matrix of x
%     b[m,m] = between class covariance matrix of x [default = I]
%     n is the number of columns in output matrix A [default = M]
%     c[m,r] specifies the first few columns of A to be predefined values [default = null)
%
% Outputs:
%     a[m,n] is the transformation matrix: y=a'*x
%     f[n,1] gives the incremental gain in f value for successive columns of A
%     B(n,n) gives the between-class covariance matrix of y
%     W[n,n] gives the within-class covariance matrix of y

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: ldatrace.m 713 2011-10-16 14:45:43Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

m=size(b,1);    % dimension of data vectors
if nargin<4
    r=0;
    if nargin<3
        n=m;
        if nargin<2
            w=eye(m);
        end
    end
else
    r=size(c,2);    % number of columns that are pre-specified
end
if r
    if n>r          % need to find additional vectors
        g=chol(w);
        v=g\null(c'*g');
        [p,l,q]=svd(v'*b*v);
        a(:,r+1:n)=v*p(:,1:n-r);
        a(:,1:r)=c;
    else
        a=c;        % no new vectors to find
    end
    if nargout>1
        ari=a/triu(qr(chol(a'*w*a))); % matrix a must be of full rank
        f=diag(ari'*b*ari);
    end
else
    [g,d]=eig(b,w,'qz');
    [ds,is]=sort(-diag(d));
    a=g(:,is(1:n));
    if nargout>1
        f=-ds(1:n);
    end
end
if nargout > 2
    B=a'*b*a;
    if nargout > 3
        W=a'*w*a;
    end

end


function [rwproc] = ranwalk(npoints, p)
% RANWALK generate and plot a trajectory of a random walk which
%    starts at 0, jumps up with probability p and down with
%    probability 1-p
%
% [rwproc] = ranwalk(npoints, p)
%
% Inputs: npoints - length of the trajectory
%         p - probability of the jump upwards
%
% Outputs: rwproc - trajectory of the process

% Authors: R.Gaigalas, I.Kaj
% v1.2 07-Oct-02

  % default parameter values
  if (nargin==0)
    npoints = 100;
    p = 0.5;
  end

  % generate jumps and sum up
  rwproc = [0 cumsum(2.*(rand(1, npoints-1)<p)-1)];

  stairs(rwproc);


function fs = get_seq ( d, norm )

%*****************************************************************************80
%
%% GET_SEQ generates all positive integer D-vectors that sum to NORM.
%
%  Discussion:
%
%    This function computes a list, in reverse dictionary order, of
%    all D-vectors of positive values that sum to NORM.
%
%    For example, fs = get_seq ( 3, 5 ) returns
%
%      3  1  1
%      2  2  1
%      2  1  2
%      1  3  1
%      1  2  2
%      1  1  3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 May 2010
%
%  Author:
%
%    Original MATLAB version by Florian Heiss, Viktor Winschel.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Florian Heiss, Viktor Winschel,
%    Likelihood approximation by numerical integration on sparse grids,
%    Journal of Econometrics,
%    Volume 144, 2008, pages 62-80.
%
%  Parameters:
%
%    Input, integer D, the dimension.
%
%    Input, integer NORM, the value that each row must sum to.
%    NORM must be at least D.
%
%    Output, integer FS(*,D).  Each row of FS represents one vector
%    with all elements positive and summing to NORM.
%
  if ( norm < d )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GET_SEQ - Fatal error!\n' );
    fprintf ( 1, '  NORM = %d < D = %d.\n', norm, d );
    error ( 'GET_SEQ - Fatal error!' );
  end

  seq = zeros ( 1, d );
%
%  The algorithm is written to work with vectors whose minimum value is
%  allowed to be zero.  So we subtract D from NORM at the beginning and
%  then increment the result vectors by 1 at the end!
%
  a = norm - d;
  seq(1) = a;
  fs = seq;
  c = 1;

  while ( seq (d) < a )

    if ( c == d )
      for i = (c-1) : -1 : 1
        c = i;
        if ( seq(i) ~= 0 )
          break
        end
      end
    end

    seq(c) = seq(c) - 1;
    c = c + 1;
    seq(c) = a - sum ( seq(1:(c-1)) );

    if ( c < d )
      seq((c+1):d) = zeros ( 1, d - c );
    end

    fs = [ fs; seq ];

  end
%
%  Increment all entries by 1.
%
  fs = fs + 1;

  return
end


function angVal=angleBetweenLineAndPlane(a,n)
%%ANGLEBETWEENLINEANDPLANE Given a line specified in parameteric form as
%       x=x0+a*t, where t is the scalar parametric parameter, and a plane
%       given in parameteric form as x'*n=d, return the (acute) angle
%       between the line and the plane.
%
%INPUTS: a The xDimX1 parameteric slope vector of the line.
%        n An xDimX1 normal to the plane. If this is omitted or an empty
%          matrix is passed, then n is a vector that is all zero except for
%          the last element, which is 1. In 3D that is equivalent to using
%          the x-y plane.
%
%OUTPUTS: angVal The angle between the line and the plane given in radians.
%
%This just uses the identity that
%sin(angVal)=abs(dot(a,n)/(norm(a)*norm(n)))
%
%EXAMPLE:
%An easy test for the function is to use the x-y plane and just have a line
%in the x-z plane defined at a specified angle to the x axis. In that
%instance, the result of this function can be quickly compared ot the
%truth.
% n=[0;0;1];
% theta=pi/3;
% a=[cos(theta);0;sin(theta)];
% AbsErr=abs(angleBetweenLineAndPlane(a,n)-theta)
%In this instance the absolute error is 0.
%
%July 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

angVal=asin(abs(dot(a,n)/(norm(a)*norm(n))));

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 [fdx, fdy, fdz] = calcFdr(N, directionFlag)
%CALCFDR Function who's
%diagonals are the k-space representation of the image-domain differencing
%operator. See "vx" in the reference below.
%
%   N: Size of volume
%   directionFlag: 'forward' or 'backward', direction of the
%   differentiation.
%
%   Code refractored from Berkin Bilgic's scripts: "script_Laplacian_unwrap_Sharp_Fast_TV_gre3D.m"
%   and "script_Laplacian_unwrap_Sharp_Fast_TV_gre3D.m"
%   Original source: https://martinos.org/~berkin/software.html
%
%   Original reference:
%   Bilgic et al. (2014), Fast quantitative susceptibility mapping with
%   L1-regularization and automatic parameter selection. Magn. Reson. Med.,
%   72: 1444-1459. doi:10.1002/mrm.25029
%

    [k2,k1,k3] = meshgrid(0:N(2)-1, 0:N(1)-1, 0:N(3)-1);

    switch directionFlag
        case 'forward'
            fdx = 1 - exp(-2*pi*1i*k1/N(1));
            fdy = 1 - exp(-2*pi*1i*k2/N(2));
            fdz = 1 - exp(-2*pi*1i*k3/N(3));
        case 'backward'
            fdx = -1 + exp(2*pi*1i*k1/N(1));
            fdy = -1 + exp(2*pi*1i*k2/N(2));
            fdz = -1 + exp(2*pi*1i*k3/N(3));
    end

end


%% Machine Learning Online Class - Exercise 2: Logistic Regression
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the logistic
%  regression exercise. You will need to complete the following functions
%  in this exericse:
%
%     sigmoid.m
%     costFunction.m
%     predict.m
%     costFunctionReg.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% Load Data
%  The first two columns contains the exam scores and the third column
%  contains the label.

data = load('ex2data1.txt');
X = data(:, [1, 2]); y = data(:, 3);

%% ==================== Part 1: Plotting ====================
%  We start the exercise by first plotting the data to understand the
%  the problem we are working with.

fprintf(['Plotting data with + indicating (y = 1) examples and o ' ...
         'indicating (y = 0) examples.\n']);

plotData(X, y);

% Put some labels
hold on;
% Labels and Legend
xlabel('Exam 1 score')
ylabel('Exam 2 score')

% Specified in plot order
legend('Admitted', 'Not admitted')
hold off;

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ============ Part 2: Compute Cost and Gradient ============
%  In this part of the exercise, you will implement the cost and gradient
%  for logistic regression. You neeed to complete the code in
%  costFunction.m

%  Setup the data matrix appropriately, and add ones for the intercept term
[m, n] = size(X);

% Add intercept term to x and X_test
X = [ones(m, 1) X];

% Initialize fitting parameters
initial_theta = zeros(n + 1, 1);

% Compute and display initial cost and gradient
[cost, grad] = costFunction(initial_theta, X, y);

fprintf('Cost at initial theta (zeros): %f\n', cost);
fprintf('Gradient at initial theta (zeros): \n');
fprintf(' %f \n', grad);

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ============= Part 3: Optimizing using fminunc  =============
%  In this exercise, you will use a built-in function (fminunc) to find the
%  optimal parameters theta.

%  Set options for fminunc
options = optimset('GradObj', 'on', 'MaxIter', 400);

%  Run fminunc to obtain the optimal theta
%  This function will return theta and the cost
[theta, cost] = ...
	fminunc(@(t)(costFunction(t, X, y)), initial_theta, options);

% Print theta to screen
fprintf('Cost at theta found by fminunc: %f\n', cost);
fprintf('theta: \n');
fprintf(' %f \n', theta);

% Plot Boundary
plotDecisionBoundary(theta, X, y);

% Put some labels
hold on;
% Labels and Legend
xlabel('Exam 1 score')
ylabel('Exam 2 score')

% Specified in plot order
legend('Admitted', 'Not admitted')
hold off;

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ============== Part 4: Predict and Accuracies ==============
%  After learning the parameters, you'll like to use it to predict the outcomes
%  on unseen data. In this part, you will use the logistic regression model
%  to predict the probability that a student with score 45 on exam 1 and
%  score 85 on exam 2 will be admitted.
%
%  Furthermore, you will compute the training and test set accuracies of
%  our model.
%
%  Your task is to complete the code in predict.m

%  Predict probability for a student with score 45 on exam 1
%  and score 85 on exam 2

prob = sigmoid([1 45 85] * theta);
fprintf(['For a student with scores 45 and 85, we predict an admission ' ...
         'probability of %f\n\n'], prob);

% Compute accuracy on our training set
p = predict(theta, X);

fprintf('Train Accuracy: %f\n', mean(double(p == y)) * 100);

fprintf('\nProgram paused. Press enter to continue.\n');
pause;


function varargout = createRhombododecahedron()
%CREATERHOMBODODECAHEDRON Create a 3D mesh representing a rhombododecahedron
%
%   [V, E, F] = createRhombododecahedron
%   V is a 14-by-3 array with vertex coordinate,
%   E is a 12-by-2 array containing indices of neighbour vertices,
%   F is a 8-by-3 array containing vertices array of each face.
%
%   [V, F] = createRhombododecahedron;
%   Returns only the vertices and the face vertex indices.
%
%   MESH = createRhombododecahedron;
%   Returns the data as a mesh structure, with fields 'vertices', 'edges'
%   and 'faces'.
%
%   Example
%   [v, e, f] = createRhombododecahedron;
%   drawMesh(v, f);
%
%
%   See also
%   meshes3d, drawMesh

%   ---------
%   author : David Legland
%   e-mail: david.legland@inra.fr
%   INRA - TPV URPOI - BIA IMASTE
%   created the 10/02/2005.
%

%   HISTORY
%   04/01/2007: remove unused variables

nodes = [0 0 2;...
    1 -1 1;1 1 1;-1 1 1;-1 -1 1;...
    2 0 0;0 2 0;-2 0 0;0 -2 0;...
    1 -1 -1;1 1 -1;-1 1 -1;-1 -1 -1;...
    0 0 -2];

edges = [...
    1 2;1 3;1 4;1 5;...
    2 6;2 9;3 6;3 7;4 7;4 8;5 8;5 9;...
    6 10;6 11;7 11;7 12;8 12;8 13;9 10;9 13; ...
    10 14;11 14;12 14;13 14];

faces = [...
    1 2 6 3;...
    1 3 7 4;...
    1 4 8 5;...
    1 5 9 2;...
    2 9 10 6;...
    3 6 11 7;...
    4 7 12 8;...
    5 8 13 9;...
    6 10 14 11;...
    7 11 14 12;...
    8 12 14 13;...
    9 13 14 10];

% format output
varargout = formatMeshOutput(nargout, nodes, edges, faces);


  function distance = penalty_distance(offsets, sizes)
%|function distance = penalty_distance(offsets, sizes)
%|
%| convert scalar offsets to Euclidean distances to neighbors,
%| with the exception that '0' is mapped to 1 for identity case
%|
%| in
%|	offsets		[MM]
%|	sizes		[1 ndim]
%| out
%|	distance	[MM 1]
%|
%| Copyright 2006-12-6, Jeff Fessler, University of Michigan

if nargin == 1 && streq(offsets, 'test'), penalty_distance_test, return, end
if nargin < 2, ir_usage, end

displace = penalty_displace(offsets(:), sizes);
distance = sqrt(sum(displace.^2, 2));
distance(offsets == 0) = 1; % trick: for identity case

%
% penalty_distance_test()
%
function penalty_distance_test

nx = 100; ny = 80; % 2d
[ix iy] = ndgrid(-2:2, -2:2);
offsets = col(ix + iy * nx);
dis = penalty_distance(offsets, [nx ny]);
tru = sqrt(ix(:).^2 + iy(:).^2);
tru(ix == 0 & iy == 0) = 1; % trick: to match identity case
jf_equal(dis, tru)

nx = 10; ny = 8; nz = 7; % 3d
[ix iy iz] = ndgrid(-2:2, -2:2, -2:2);
offsets = col(ix + iy * nx + iz * nx * ny);
dis = penalty_distance(offsets, [nx ny nz]);
tru = sqrt(ix(:).^2 + iy(:).^2 + iz(:).^2);
tru(ix == 0 & iy == 0 & iz == 0) = 1; % trick: to match identity case
jf_equal(dis, tru)


function pn = line_par_point_near_3d ( f, g, h, x0, y0, z0, p )

%*****************************************************************************80
%
%% LINE_PAR_POINT_NEAR_3D: nearest point on parametric line to given point, 3D.
%
%  Discussion:
%
%    The parametric form of a line in 3D is:
%
%      X = X0 + F * T
%      Y = Y0 + G * T
%      Z = Z0 + H * T
%
%    We may normalize by choosing F*F + G*G + H*H = 1, and F nonnegative.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Adrian Bowyer, John Woodwark,
%    A Programmer's Geometry,
%    Butterworths, 1983,
%    ISBN: 0408012420.
%
%  Parameters:
%
%    Input, real F, G, H, X0, Y0, Z0, the parametric
%    line parameters.
%
%    Input, real P(3,1), the point whose distance from the line is
%    to be measured.
%
%    Output, real PN(3,1), the point on the parametric line which
%    is nearest to P.
%
  t = ( f * ( p(1) - x0 ) + g * ( p(2) - y0 ) + h * ( p(3) - z0 ) ) ...
    / ( f * f + g * g + h * h );

  pn(1,1) = x0 + t * f;
  pn(2,1) = y0 + t * g;
  pn(3,1) = z0 + t * h;

  return
end


function [x y]=bresenham(x1,y1,x2,y2)

%Matlab optmized version of Bresenham line algorithm. No loops.
%Format:
%               [x y]=bham(x1,y1,x2,y2)
%
%Input:
%               (x1,y1): Start position
%               (x2,y2): End position
%
%Output:
%               x y: the line coordinates from (x1,y1) to (x2,y2)
%
%Usage example:
%               [x y]=bham(1,1, 10,-5);
%               plot(x,y,'or');
x1=round(x1); x2=round(x2);
y1=round(y1); y2=round(y2);
dx=abs(x2-x1)
dy=abs(y2-y1)
steep=abs(dy)>abs(dx)
if steep t=dx;dx=dy;dy=t; end

%The main algorithm goes here.
if dy==0
    q=zeros(dx+1,1);
else
    q=[0;diff(mod([floor(dx/2):-dy:-dy*dx+floor(dx/2)]',dx))>=0]
end

%and ends here.

if steep
    if y1<=y2 y=[y1:y2]'; else y=[y1:-1:y2]'; end
    if x1<=x2 x=x1+cumsum(q);else x=x1-cumsum(q); end
else
    if x1<=x2 x=[x1:x2]'; else x=[x1:-1:x2]'; end
    if y1<=y2 y=y1+cumsum(q);else y=y1-cumsum(q); end
end


%% Creating one filter
% In this demo, we will show how to build some filters in spatial domain or
% Fourier domain. Gabor, Morlet and Gaussian filters are pointed out.
%
%% Reviewed functions
% The following function will be demonstrated:
%%
%
% # gabor_2d
% # gaussian_2d
% # morlet_2d_noDC
% # morlet_2d_pyramid
%
%% Demonstration
%
% Here are shown 4 examples of filters:

N=50;
M=50;
sigma=10;
slant=3;
theta=pi/3;
xi=2;

gab = gabor_2d(N, M, sigma, slant, xi, theta);
morlet = morlet_2d_noDC(N, M, sigma, slant, xi, theta);
morlet2 = morlet_2d_pyramid(N, M, sigma, slant, xi, theta);
gaussian = gaussian_2d(N, M, sigma);
subplot(2,2,1)
imagesc(real(gab));
axis off
title('Real part of Gabor wavelet in Fourier domain')
subplot(2,2,2)
imagesc(real(morlet));
axis off
title('Real part of Morlet wavelet in Fourier domain')
subplot(2,2,3)
imagesc(real(morlet2));
axis off
title('Real part of Morlet wavelet in Spatial domain')
subplot(2,2,4)
imagesc(gaussian);
axis off
title('Gaussian wavelet')


function [ x, seed ] = truncated_normal_ab_sample ( mu, sigma, a, b, seed )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_AB_SAMPLE samples the truncated Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real MU, SIGMA, the mean and standard deviation of the
%    parent Normal distribution.
%
%    Input, real A, B, the lower and upper truncation limits.
%
%    Input/output, integer SEED, a seed for the random number
%    generator.
%
%    Output, real X, a sample of the PDF.
%
  alpha = ( a - mu ) / sigma;
  beta = ( b - mu ) / sigma;

  alpha_cdf = normal_01_cdf ( alpha );
  beta_cdf = normal_01_cdf ( beta );

  [ u, seed ] = r8_uniform_01 ( seed );
  xi_cdf = alpha_cdf + u * ( beta_cdf - alpha_cdf );
  xi = normal_01_cdf_inv ( xi_cdf );

  x = mu + sigma * xi;

  return
end


%ZNCC  Normalized cross correlation
%
% M = ZNCC(I1, I2) is the zero-mean normalized cross-correlation between the
% two equally sized image patches I1 and I2.  The result M is a scalar in
% the interval -1 to 1 that indicates similarity.  A value of 1 indicates
% identical pixel patterns.
%
% Notes::
% - The ZNCC similarity measure is invariant to affine changes in image
%   intensity (brightness offset and scale).
%
% See also NCC, SAD, SSD, ISIMILARITY.

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

function m = zncc(w1, w2)

	w1 = w1 - mean(w1(:));
	w2 = w2 - mean(w2(:));

	denom = sqrt( sum(sum(w1.^2))*sum(sum(w2.^2)) );

	if denom < 1e-10,
		m = 0;
	else
		m = sum(sum((w1.*w2))) / denom;
	end


function [J, grad] = lrCostFunction(theta, X, y, lambda)
%LRCOSTFUNCTION Compute cost and gradient for logistic regression with
%regularization
%   J = LRCOSTFUNCTION(theta, X, y, lambda) computes the cost of using
%   theta as the parameter for regularized logistic regression and the
%   gradient of the cost w.r.t. to the parameters.

% Initialize some useful values
m = length(y); % number of training examples

% You need to return the following variables correctly
J = 0;
grad = zeros(size(theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost of a particular choice of theta.
%               You should set J to the cost.
%               Compute the partial derivatives and set grad to the partial
%               derivatives of the cost w.r.t. each parameter in theta
%
% Hint: The computation of the cost function and gradients can be
%       efficiently vectorized. For example, consider the computation
%
%           sigmoid(X * theta)
%
%       Each row of the resulting matrix will contain the value of the
%       prediction for that example. You can make use of this to vectorize
%       the cost function and gradient computations.
%
% Hint: When computing the gradient of the regularized cost function,
%       there're many possible vectorized solutions, but one solution
%       looks like:
%           grad = (unregularized gradient for logistic regression)
%           temp = theta;
%           temp(1) = 0;   % because we don't add anything for j = 0
%           grad = grad + YOUR_CODE_HERE (using the temp variable)
%

h = sigmoid(X * theta);
J = sum(-y .* log(h) - (1 - y) .* log(1 - h)) / m;
% regularization for cost function
J = J + (lambda / (2 * m)) * sum(theta(2:end) .^ 2);
grad = (1 / m) * X' * (h - y);
% regularization for gradient values
grad = grad + (lambda / m) * [0; theta(2:end)];

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

grad = grad(:);

end


function varargout = rotation3dToEulerAngles(mat, varargin)
%ROTATION3DTOEULERANGLES Extract Euler angles from a rotation matrix.
%
%   [PHI, THETA, PSI] = rotation3dToEulerAngles(MAT)
%   Computes Euler angles PHI, THETA and PSI (in degrees) from a 3D 4-by-4
%   or 3-by-3 rotation matrix.
%
%   ANGLES = rotation3dToEulerAngles(MAT)
%   Concatenates results in a single 1-by-3 row vector. This format is used
%   for representing some 3D shapes like ellipsoids.
%
%   ... = rotation3dToEulerAngles(MAT, CONVENTION)
%   CONVENTION specifies the axis rotation sequence.
%   Supported conventions are: 'ZYX', 'ZYZ'. Default is 'ZYX'
%
%   Example
%   rotation3dToEulerAngles
%
%   References
%   Code from Graphics Gems IV on euler angles
%   http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
%   Modified using explanations in:
%   http://www.gregslabaugh.name/publications/euler.pdf
%
%   See also
%   transforms3d, rotation3dAxisAndAngle, createRotation3dLineAngle,
%   eulerAnglesToRotation3d
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-08-11,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

p = inputParser;
validStrings = {'ZYX','ZYZ'};
addOptional(p,'convention','ZYX',@(x) any(validatestring(x,validStrings)));
parse(p,varargin{:});
convention=p.Results.convention;

% conversion from radians to degrees
k = 180 / pi;

switch convention
    case 'ZYX'
        % extract |cos(theta)|
        cy = hypot(mat(1,1), mat(2,1));
        % avoid dividing by 0
        if cy > 16*eps
            % normal case: theta <> 0
            phi   = k * atan2( mat(2,1), mat(1,1));
            theta = k * atan2(-mat(3,1), cy);
            psi   = k * atan2( mat(3,2), mat(3,3));
        else
            %
            phi   = 0;
            theta = k * atan2(-mat(3,1), cy);
            psi   = k * atan2(-mat(2,3), mat(2,2));
        end
    case 'ZYZ'
        cy = hypot(mat(3,2), mat(3,1));
        if cy > 16*eps
            phi   = k * -atan2(mat(2,3), -mat(1,3));
            theta = k * -atan2(cy, mat(3,3));
            psi   = k * -atan2(mat(3,2), mat(3,1));
        else
            phi   = 0;
            theta = k * atan2(cy, mat(3,3));
            psi   = k * atan2(mat(2,1), mat(2,2));
        end
end

% format output arguments
if nargout <= 1
    % one array
    varargout{1} = [phi theta psi];
else
    % three separate arrays
    varargout = {phi, theta, psi};
end


function STATS=mwwtest(x1,x2)
% Mann-Whitney-Wilcoxon non parametric test for two unpaired groups.
% This file execute the non parametric Mann-Whitney-Wilcoxon test to evaluate the
% difference between unpaired samples. If the number of combinations is less than
% 20000, the algorithm calculates the exact ranks distribution; else it
% uses a normal distribution approximation. The result is not different from
% RANKSUM MatLab function, but there are more output informations.
% There is an alternative formulation of this test that yields a statistic
% commonly denoted by U. Also the U statistic is computed.
%
% Syntax: 	STATS=MWWTEST(X1,X2)
%
%     Inputs:
%           X1 and X2 - data vectors.
%     Outputs:
%           - T and U values and p-value when exact ranks distribution is used.
%           - T and U values, mean, standard deviation, Z value, and p-value when
%           normal distribution is used.
%        If STATS nargout was specified the results will be stored in the STATS
%        struct.
%
%      Example:
%
%         X1=[181 183 170 173 174 179 172 175 178 176 158 179 180 172 177];
%
%         X2=[168 165 163 175 176 166 163 174 175 173 179 180 176 167 176];
%
%           Calling on Matlab the function: mwwtest(X1,X2)
%
%           Answer is:
%
% MANN-WHITNEY-WILCOXON TEST
%
%                        Group_1    Group_2
%                        _______    _______
%
%     Numerosity          15         15
%     Sum_of_Rank_W      270        195
%     Mean_Rank           18         13
%     Test_variable_U     75        150
%
% Sample size is large enough to use the normal distribution approximation
%
%     Mean       SD        Z       p_value_one_tail    p_value_two_tails
%     _____    ______    ______    ________________    _________________
%
%     112.5    24.047    1.5386    0.061947            0.12389
%
%           Created by Giuseppe Cardillo
%           giuseppe.cardillo-edta@poste.it
%
% To cite this file, this would be an appropriate format:
% Cardillo G. (2009). MWWTEST: Mann-Whitney-Wilcoxon non parametric test for two unpaired samples.
% http://www.mathworks.com/matlabcentral/fileexchange/25830
%Input Error handling
p = inputParser;
addRequired(p,'x1',@(x) validateattributes(x,{'numeric'},{'row','real','finite','nonnan','nonempty'}));
addRequired(p,'x2',@(x) validateattributes(x,{'numeric'},{'row','real','finite','nonnan','nonempty'}));
parse(p,x1,x2);
%set the basic parameter
n1=length(x1); n2=length(x2); NP=n1*n2; N=n1+n2; N1=N+1; k=min([n1 n2]);
[A,B]=tiedrank([x1(:); x2(:)]); %compute the ranks and the ties
R1=A(1:n1); R2=A(n1+1:end);
T1=sum(R1); T2=sum(R2);
U1=NP+(n1*(n1+1))/2-T1; U2=NP-U1;
disp('MANN-WHITNEY-WILCOXON TEST')
disp(' ')
disp(table([n1;T1;T1/n1;U1],[n2;T2;T2/n2;U2],...
    'VariableNames',{'Group_1' 'Group_2'},...
    'RowNames',{'Numerosity' 'Sum_of_Rank_W' 'Mean_Rank' 'Test_variable_U'}))
if nargout
    STATS.n=[n1 n2];
    STATS.W=[T1 T2];
    STATS.mr=[T1/n1 T2/n2];
    STATS.U=[U1 U2];
end
if round(exp(gammaln(N1)-gammaln(k+1)-gammaln(N1-k))) > 20000
    mU=NP/2;
    if B==0
        sU=realsqrt(NP*N1/12);
    else
        sU=realsqrt((NP/(N^2-N))*((N^3-N-2*B)/12));
    end
    Z1=(abs(U1-mU)-0.5)/sU;
    p=1-normcdf(Z1); %p-value
    disp('Sample size is large enough to use the normal distribution approximation')
    disp(' ')
    disp(table(mU,sU,Z1,p,2*p,'VariableNames',{'Mean' 'SD' 'Z' 'p_value_one_tail' 'p_value_two_tails'}))
    if nargout
        STATS.method='Normal approximation';
        STATS.mU=mU;
        STATS.sU=sU;
        STATS.Z=Z1;
        STATS.p=[p 2*p];
    end
else
    disp('Sample size is small enough to use the exact Mann-Whitney-Wilcoxon distribution')
    disp(' ')
    if n1<=n2
        w=T1;
    else
        w=T2;
    end
    pdf=sum(nchoosek(A,k),2);
    P = [sum(pdf<=w) sum(pdf>=w)]./length(pdf);
    p = min(P);
    disp(table(w,p,2*p,'VariableNames',{'W' 'p_value_one_tail' 'p_value_two_tails'}))
    if nargout
        STATS.method='Exact distribution';
        STATS.T=w;
        STATS.p=[p 2*p];
    end
end
disp(' ')


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

% Symmetry

% 	Even symmetry

syms x t k T
w=2*pi/T;
c1=(2/T)*int(x*sin(k*w*t),t,-T/2,0);

c2=(2/T)*int(x*sin(k*w*t),t,0,T/2);
c=c1+c2

t0=-2;
T=4;
w=2*pi/T;
syms t
x=t^2;
k=-5:5;
a=(1/T)*int(x*exp(-j*k*w*t),t,t0,t0+T);
stem(k,eval(a))
legend('a_k')

% 	Odd symmetry
figure
syms x t k T
w=2*pi/T;
b1=(2/T)*int(-x*cos(k*w*t),t,-T/2,0);

b2=(2/T)*int(x*cos(k*w*t),t,0,T/2);
b=b1+b2

t0=-2;
T=4;
w=2*pi/T;
syms t
x=t;
k=-5:5;
a=(1/T)*int(x*exp(-j*k*w*t),t,t0,t0+T);

stem(k,imag(eval(a)))
legend('a_k')


function p = multivariateGaussian(X, mu, Sigma2)
%MULTIVARIATEGAUSSIAN Computes the probability density function of the
%multivariate gaussian distribution.
%    p = MULTIVARIATEGAUSSIAN(X, mu, Sigma2) Computes the probability
%    density function of the examples X under the multivariate gaussian
%    distribution with parameters mu and Sigma2. If Sigma2 is a matrix, it is
%    treated as the covariance matrix. If Sigma2 is a vector, it is treated
%    as the \sigma^2 values of the variances in each dimension (a diagonal
%    covariance matrix)
%

k = length(mu);

if (size(Sigma2, 2) == 1) || (size(Sigma2, 1) == 1)
    Sigma2 = diag(Sigma2);
end

X = bsxfun(@minus, X, mu(:)');
p = (2 * pi) ^ (- k / 2) * det(Sigma2) ^ (-0.5) * ...
    exp(-0.5 * sum(bsxfun(@times, X * pinv(Sigma2), X), 2));

end


%% Runge-Kutta 2
%
% for solving
%
% $\frac{dv}{dt}=-\alpha (t)v+\beta (t)$
%
% where $\alpha (t) = \frac{3t}{1-t}$ and $\beta (t)=2(1-t)^3 e^{-t}$
%
% Assuming v(0)=1.0 for the period 0<t<15

clc
clear
%close all

%% Discretization of t
ti=0;    % Initial Time
tf=150;   % Final Time

% Time step
% h = 0.2;
% h = 0.8;
% h = 1.1;
 h = 0.68;

% Discretization of t
i=1;
t(1)=ti;
while t(i)<tf
        i=i+1;
        t(i)=t(i-1)+h;
end
t(i)=tf; clear i;
n=length(t);

%% Discretization of v

v = zeros(n,1);
alpha = zeros(n,1);
beta  = zeros(n,1);
t2 = zeros(n,1);
alpha2 = zeros(n,1);
beta2  = zeros(n,1);

for i=1:n-1
    t2(i)     = t(i)+1/2*h;
    % compute variables alpha(t) and beta(t)
    alpha(i)  = 3*t(i)/(1+t(i));
    alpha2(i) = 3*t2(i)/(1+t2(i)); % alpha(t + 1/2h)
    beta(i)   = 2*(1+t(i))^3*exp(-t(i));
    beta2(i)  = 2*(1+t2(i))^3*exp(-t2(i)); % beta(t + 1/2h)
end

% initial condition (v(0))
v(1)=1;

for i=1:n-1
    % main discrete equation
    k1 = h*(-alpha(i)*v(i)+beta(i));
    k2 = h*(-alpha2(i)*(v(i)+1/2*k1)+beta2(i));
    v(i+1) = v(i)+k2;
end

%% Exact solution
v2 = zeros(n,1);
for i=1:n
     v2(i) = exp(-t(i))*(1+t(i))^3;
end
error=v2-v;
figure
plot(t,v,'.',t,v2,'-')
%figure
%plot(t,error,'.')


function [khs, ghs, def] = HashinShtrikmanModulus(C,Ko,Go)
% Hashin Shtrikman moduli
%
% Description
% The equations are from Peselnick and Meister 1965 through Watt and
% Peselnick 1980 which are based on Hashin and Shtrikman 1963 JMB 8/2013
%
% Syntax:
%
%   [khs, ghs, def] = HashinShtrikmanModulus(C,Ko,Go)
%
% Input
%  C  - @stiffnessTensor
%  Ko - bulk modulus of the reference isotropic material
%  Go - shear modulus of the reference isotropic material
%
% Output
%  khs - Hashin Shtrikman bulk modulus
%  ghs - Hashin Shtrikman shear modulus
%  def - +1 positive definite, -1 for negative definite R
%

% ensure K0 and G0 have same size
if numel(Go) == 1, Go = repmat(Go,size(Ko)); end

% the isotropic compliances elements (eq. 10-11 Watt Peselnick 1980)
alpha = -3 ./ (3*Ko + 4*Go);
beta = -3 * (Ko + 2*Go) ./ ( 5*Go .* (3*Ko + 4*Go) );
gamma = (alpha - 3 * beta) ./ 9;

% define the isotropic tensor corresponding to Ko and Go
Co = 2 * Go .* stiffnessTensor.eye + (Ko - 2/3 * Go) .* dyad(tensor.eye,tensor.eye);

% take difference between anisotropic and isotropic matrices and take
% inverse to get the residual compliance matrix (eq 4 & 8 Watt Peselnick 1980)
R = C - Co;
H = inv(R);

% Subtract the isotropic compliances from the residual compliance matrix
% and invert back to moduli space.  The matrix B is the moduli matrix that
% when multiplied by the strain difference between the reference isotropic
% response and the isotropic response of the actual material gives the
% average stres <pij> (eq 15 - 16 Watt Peselnick 1980).
A = H - beta .* complianceTensor.eye - gamma .* dyad(tensor.eye,tensor.eye);
B = inv(A);

% now perform the averaging of the B matrix to get two numbers (B1 and B2)
% that are related to the two isotropic moduli - perturbations of the
% reference values (eq 21 and 22 Watt Peselnick 1980)
sB1 = EinsteinSum(B,[-1 -1 -2 -2]);
sB2 = EinsteinSum(B,[-1 -2 -1 -2]);

B1 = (2*sB1 - sB2) / 15;
B2 = (3*sB2 - sB1) / 30;

% The Hashin-Shtrikman moduli are then calculated as changes from the
% reference isotropic body. (eq 25 & 27 Watt and Peselnick 1980)
khs = Ko + ( 3 * B1 + 2 * B2 ) ./ ( 3 + alpha .* (3 * B1 + 2 * B2) );
ghs = Go + B2 ./ ( 1 + 2 * beta .* B2 );

% The moduli are valid in two limits - minimizing or maximizing the
% anisotropic difference elastic energy. Think of it as the maximum
% positive deviations from the reference state or the maximum negative
% deviations from the reference state. These are either in the positive
% definite or negative definite regime of the matrix R. Here is a test of
% the properties of R.  A +1 is returned if positive definite, a -1 is
% retunred if negative definite.  0 returned otherwise.

def = zeros(size(R));
D = eig(R);
def(all(D>0,1)) = 1;
def(all(D<0,1)) = -1;

% isf minmax == 0 this means ...
khs(def==0) = NaN;
ghs(def==0) = NaN;


function [A, b] = sllinrega(X, Y, varargin)
%SLLINREGA Performs Augmented Multivariate Linear Regression
%
% $ Syntax $
%   - [A, b] = sllinrega(X, Y, ...)
%
% $ Arguments $
%   - X:        The sample matrix of x
%   - Y:        The sample matrix of y
%   - A:        The solved transform matrix
%   - b:        The solved shift vector
%
% $ Description $
%   - [A, b] = sllinrega(X, Y, ...) solves the regression problem given
%     by the following formula:
%           y = A * x + b
%     in least square error sense. The samples are stored in X and Y
%     in column-wise manner.
%     You can specify properties for regression as in sllinreg.
%
% $ Remarks $
%   - The implementation is based on sllinreg with an augmented
%     formulation as follows:
%           y = [A, b] * [x; 1]
%
% $ History $
%   - Created by Dahua Lin, on Sep 15th, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('sllinrega', 2);
end

if ~isnumeric(X) || ~isnumeric(Y) || ndims(X) ~= 2 || ndims(Y) ~= 2
    error('sltoolbox:invalidarg', ...
        'The X and Y should be both 2D numeric matrices');
end

%% main

% augment formulation
[dx, nx] = size(X);
Xa = [X; ones(1, nx)];

% solve
Aa = sllinreg(Xa, Y, varargin{:});
clear Xa;

% extract
A = Aa(:, 1:dx);
b = Aa(:, dx+1);


function y = compute_orthogonal_projection( x, A, C )

% compute_orthogonal_projection - computer orthogonal projection
%
%   y = compute_orthogonal_projection( x, A, C );
%
%   y is the pojection of x onto the intersection of the sets
%       { y \ <x-cj,aj>=0 }
%   where the cj and the aj are the columns of C and A.
%
%   Copyright (c) 2007 Gabriel Peyre

% compute solution as
%   y = x + A*lambda

D = (A'*A);
if abs(det(D))<1e-9
    D = D+rand(size(D))*1e-9;
end
lambda = D \ ( -A'*x + diag(A'*C) );
y = x + A*lambda;


clear all; close all;
figure(1)
% underdetermined
n=20; m=100
A=rand(n,m); b=rand(n,1);

cvx_begin;
variable x2(m)
minimize( norm(x2,2) );
subject to
A*x2 == b;
cvx_end;

cvx_begin;
variable x1(m)
minimize( norm(x1,1) );
subject to
A*x1 == b;
cvx_end;

subplot(3,1,1), bar(x2)
subplot(3,1,2), bar(x1)
subplot(3,2,5), hist(x2,40)
subplot(3,2,6), hist(x1,40)

subplot(3,1,1), axis([0 100 -.2 .2]), subplot(3,1,2), axis([0 100 -.4 .4])
subplot(3,2,5), axis([-.15 .15 0 82]), set(gca,'Ytick',[0 40 80],'Xtick',[-0.1 0 0.1])
h = findobj(gca,'Type','patch'); h.FaceColor = [0.6 0.6 0.6]; h.EdgeColor = 'k';
subplot(3,2,6), axis([-.15 .15 0 82]), set(gca,'Ytick',[0 40 80],'Xtick',[-0.1 0 0.1])
h = findobj(gca,'Type','patch'); h.FaceColor = [0.6 0.6 0.6]; h.EdgeColor = 'k';

%%
clear A
clear b
clear x
figure(2)

% overdetermined
n=500; m=100;
A=rand(n,m);
b=rand(n,1);
xdag=pinv(A)*b;

lam=[0 0.1 0.5];
for j=1:3

    cvx_begin;
    variable x(m)
    minimize( norm(A*x-b,2) + lam(j)*norm(x,1) );
    cvx_end;

    subplot(4,1,j),bar(x)
    subplot(4,3,9+j), hist(x,20)
end

for j=1:3

    subplot(4,1,j), axis([0 100 -.15 .15])
    subplot(4,3,9+j), axis([-.15 .15 0 80]), set(gca,'Ytick',[0 40 80])
    h = findobj(gca,'Type','patch');
    h.FaceColor = [0.6 0.6 0.6];
    h.EdgeColor = 'k';
end

%%
% matrix overdetermined system

clear A
clear b
clear x
figure(3)

% overdetermined
n=300; m=60; p=20;
A=rand(n,m); b=rand(n,p);

lam=[0 0.1];
for j=1:2
    cvx_begin;
    variable x(m,p)
    minimize(norm(A*x-b,2) + lam(j)*norm(x,1));
    cvx_end;
    subplot(2,1,j), pcolor(x.'), colormap(hot), colorbar
end

%%
subplot(2,1,1), set(gca,'Fontsize',[15],'Xtick',[1 20 40 60],'Ytick',[1 10 20])
subplot(2,1,2), set(gca,'Fontsize',[15],'Xtick',[1 20 40 60],'Ytick',[1 10 20])


function hsi = rgb2hsi(rgb)
%RGB2HSI Converts an RGB image to HSI.
%   HSI = RGB2HSI(RGB) converts an RGB image to HSI. The input image is
%   assumed to be of size M-by-N-by-3, where the third dimension
%   accounts for three image planes: red, green, and blue, in that
%   order. If all RGB component images are equal, the HSI conversion is
%   undefined. The input image can be of class double (with values in
%   the range [0, 1]), uint8, or uint16.
%
%   The output image, HSI, is of class double, where:
%     HSI(:,:,1) = hue image normalized to the range [0,1] by dividing
%                  all angle values by 2*pi.
%     HSI(:,:,2) = saturation image, in the range [0,1].
%     HSI(:,:,3) = intensity image, in the range [0,1].
%
%   All the conversion equations are explained in Chapter 7 of DIPUM3E.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Extract the individual component images.
rgb = im2double(rgb);
r = rgb(:,:,1);
g = rgb(:,:,2);
b = rgb(:,:,3);

% Implement the conversion equations.
num = 0.5*((r - g) + (r - b));
den = sqrt((r - g).^2 + (r - b).*(g - b));
theta = acos(num./(den + eps));

H = theta;
H(b > g) = 2*pi - H(b > g);
H = H/(2*pi);

num = min(min(r,g),b);
den = r + g + b;
den(den == 0) = eps;
S = 1 - 3.* num./den;

H(S == 0) = 0;

I = (r + g + b)/3;

% Combine all three results into an hsi image.
hsi = cat(3,H,S,I);


function dist = segment_point_dist_2d ( p1, p2, p )

%*****************************************************************************80
%
%% SEGMENT_POINT_DIST_2D: distance ( line segment, point ) in 2D.
%
%  Discussion:
%
%    A line segment is the finite portion of a line that lies between
%    two points.
%
%    The nearest point will satisfy the condition
%
%      PN = (1-T) * P1 + T * P2.
%
%    T will always be between 0 and 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), the endpoints of the line segment.
%
%    Input, real P(2,1), the point whose nearest neighbor on the line
%    segment is to be determined.
%
%    Output, real DIST, the distance from the point to the line segment.
%

%
%  If the line segment is actually a point, then the answer is easy.
%
  if ( p1(1:2,1) == p2(1:2,1) )

    t = 0.0;

  else

    bot = sum ( ( p2(1:2,1) - p1(1:2,1) ).^2 );

    t = ( p(1:2,1) - p1(1:2,1) )' * ( p2(1:2,1) - p1(1:2,1) ) / bot;

    t = max ( t, 0.0 );
    t = min ( t, 1.0 );

  end

  pn(1:2,1) = p1(1:2,1) + t * ( p2(1:2,1) - p1(1:2,1) );

  dist = sqrt ( sum ( ( pn(1:2,1) - p(1:2,1) ).^2 ) );

  return
end


function [pgx,pgy,area,perim] = minboundparallelogram(x,y,metric)
% minboundparallelogram: Compute the minimal bounding parallelogram of points in the plane
% usage: [pgx,pgy,area,perimeter] = minboundparallelogram(x,y,metric)
%
% While minboundrect allows you to choose between a minimal area or
% perimeter, it turns out that the minimal area paralellogram is not
% always unique. However, there will exist a unique parallelogram of
% minimal perimeter.
%
% arguments: (input)
%  x,y - vectors of points, describing points in the plane as
%        (x,y) pairs. x and y must be the same lengths.
%
%  metric - (OPTIONAL) - single letter character flag which
%        denotes the use of minimal area or perimeter as the
%        metric to be minimized. metric may be either 'a' or 'p',
%        capitalization is ignored. Any other contraction of 'area'
%        or 'perimeter' is also accepted.
%
%        DEFAULT: 'a'    ('area')
%
% arguments: (output)
%  pgx,pgy - 5x1 vectors of points that define the minimal
%        bounding parallelogram.
%
%  area - (scalar) area of the minimal parallelogram itself.
%
%  perimeter - (scalar) perimeter of the minimal parallelogram as found
%
%
% Example usage:
%  x = rand(50000,1);
%  y = rand(50000,1);
%  tic,[px,py,area,perim] = minboundparallelogram(x,y);toc
%
%  Elapsed time is 0.083340 seconds.
%
% [px,py]
% ans =
%       8.73336658824275e-05      3.51448852413649e-05
%           1.00002616513798       3.7428827532671e-05
%          0.999923888887252         0.999977663164396
%      -1.49425848464391e-05         0.999975379222104
%       8.73336658824275e-05      3.51448852413649e-05
%
% area
% area =
%          0.999879069698332
%
% perim
% perim =
%            3.9997581420842
%
%
% See also: minboundcircle, minboundtri, minboundrect
%
%
% Author: John D'Errico
% E-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 8/19/2010

% default for metric
if (nargin<3) || isempty(metric)
  metric = 'a';
elseif ~ischar(metric)
  error('metric must be a character flag if it is supplied.')
else
  % check for 'a' or 'p'
  metric = lower(metric(:)');
  ind = strmatch(metric,{'area','perimeter'});
  if isempty(ind)
    error('metric does not match either ''area'' or ''perimeter''')
  end

  % just keep the first letter.
  metric = metric(1);
end

% preprocess data
x=x(:);
y=y(:);

% not many error checks to worry about
n = length(x);
if n~=length(y)
  error('x and y must be the same sizes')
end

% start out with the convex hull of the points to
% reduce the problem dramatically. Note that any
% points in the interior of the convex hull are
% never needed, so we drop them.
if n>3
  edges = convhull(x,y);  % 'Pp' will silence the warnings

  % exclude those points inside the hull as not relevant
  % also sorts the points into their convex hull as a
  % closed polygon

  x = x(edges(1:(end-1)));
  y = y(edges(1:(end-1)));

  % probably fewer points now, unless the points are fully convex
  nedges = length(x) - 1;

elseif n > 1
  % n must be 2 or 3
  nedges = n;
  x(end+1) = x(1);
  y(end+1) = y(1);
else
  % n must be 0 or 1
  nedges = n;
end

% now we must find the bounding parallelogram of those
% that remain.

% special case small numbers of points. If we trip any
% of these cases, then we are done, so return.
switch nedges
  case 0
    % empty begets empty
    pgx = [];
    pgy = [];
    area = [];
    perimeter = [];
    return
  case 1
    % with one point, the rect is simple.
    pgx = repmat(x,1,5);
    pgy = repmat(y,1,5);
    area = 0;
    perimeter = 0;
    return
  case 2
    % only two points. also simple.
    pgx = x([1 2 2 1 1]);
    pgy = y([1 2 2 1 1]);
    area = 0;
    perimeter = 2*sqrt(diff(x).^2 + diff(y).^2);
    return
end
% 3 or more points.

% will need a 2x2 rotation matrix through an angle theta
Rmat = @(theta) [cos(theta) sin(theta);-sin(theta) cos(theta)];

% get the angle of each edge of the hull polygon.
nx = length(x);
ind1 = 1:nx;
ind2 = circshift(ind1,[0 -1]);
edgeangles = atan2(y(ind2) - y(ind1),x(ind2) - x(ind1));
% move the angle to be in [0,pi).
edgeangles = mod(edgeangles,pi);

% now just check each edge of the convex hull
nang = length(edgeangles);
area = inf;
perim = inf;
met = inf;
xy = [x,y];
for i = 1:nang
  % line that defines the base of the parallelogram
  p1 = xy(i,:);

  % we will rotate and translate the points so this
  % edge lies on the x axis. The rotation matrix is...
  rot = Rmat(-edgeangles(i));
  xyr = (xy - repmat(p1,nx,1))*rot;

  % the height of the parallelogram is
  if max(xyr(:,2)) >= max(-xyr(:,2))
    pgheight = max(xyr(:,2));
  else
    pgheight = min(xyr(:,2));
  end

  % rotate everything, but keep the angles in the interval [0,pi)
  anglesr = mod(edgeangles - edgeangles(i),pi);

  % what are the smallest and largest possible angles of the
  % remaining edges? This will define the limits of where the
  % secondary sides of the bounding parallelogram may lie.
  anglesr(i) = [];
  anglesr(anglesr == 0) = [];
  anglebounds = [min(anglesr), max(anglesr)];

  % use fminbnd to search over the family of parallelograms with
  % the given base.
  sideang = fminbnd(@(ang) pgramobj(ang,xyr,pgheight),anglebounds(1),anglebounds(2));

  % get the final parallelogram
  [pmin,amin,pgxy] = pgramobj(sideang,xyr,pgheight);

  if metric=='a'
    M_i = amin;
  else
    M_i = pmin;
  end

  % A new metric value for the current interval.
  % Is it better than the previous best?
  if M_i < met
    % keep this one
    met = M_i;
    area = amin;
    perim = pmin;

    % recover the parallelogram in the original coordinate system
    pgxy = pgxy*rot' + repmat(p1,5,1);

    pgx = pgxy(:,1);
    pgy = pgxy(:,2);

%    plot(x,y,'kx',pgx,pgy,'-ro')
%    title(num2str([perim,area]))
%    axis equal
%    pause

  end
end

% all done
% plot(x,y,'kx',pgx,pgy,'-ro')
% title(num2str([perim,area]))
% axis equal

% =================================================================
function [perim,area,pgxy] = pgramobj(ang,xyr,pgheight)
%  For a given angle (in radians) computes the bounding
%  parallelogram where the base is assumed to lie on the
%  x axis. Thus the upper face must be parallel to the x axis.

% ang will always be in the open interval (0,pi)

% The vector that points along the sides will be
sidevec = [cos(ang),sin(ang)];

% for this value of ang, what is the normal vector to
% that side of the parallelogram? It always points in
% the positive x direction, by the way it is constructed.
nvec = [sin(ang);-cos(ang)];

% What is the left most point that a side will hit against?
% The rightmost? get that information from a dot product of
% the points with the normal vector of the sides.
dp = xyr*nvec;
leftmost = min(dp);
rightmost = max(dp);

% a point on the line that contains the left side of the
% parallelogram is
leftpoint = leftmost*nvec.';

% and the right hand side line passes through this point.
rightpoint = rightmost*nvec.';

% we can define each line by the parametric equations
%  Pleft(t) = leftpoint + t*sidevec
%  Pright(t) = rightpoint + s*sidevec
%
% now find the values of s and t, such that those lines
% intersect the x axis (thus y == 0)
t0 = -leftpoint(2)./sidevec(2);
s0 = -rightpoint(2)./sidevec(2);

% the actual x axis intercepts are at
x1 = leftpoint(1) + t0*sidevec(1);
x2 = rightpoint(1) + s0*sidevec(1);

% the length of a side of the parallelogram is given by the
% definition of the trig function from triangle sides.
sidelength = pgheight./cos(pi/2 - ang);
baselength = abs(x2 - x1);

% so the perimeter of the parallellogram is given by
perim = 2*(abs(sidelength) + baselength);

% don't bother to do these unless necessary
if nargout > 1
  % the area is also trivial to compute, as
  area = abs(pgheight)*baselength;

  % and the vertices of the parallelogram are
  pgxy = [[x1,0]; [x2,0]; ...
    [x2,0] + sidelength*sidevec ; ...
    [x1,0] + sidelength*sidevec ; ...
    [x1,0]];

end


function [xi,w]=thirdOrderCrossPolyCubPoints(numDim,algorithm)
%%THIRDORDERCROSSPOLYCUBPOINTS Generate third order cubature points for
%               integration over the region sum(abs(x))<=1 with weighting
%               function w(x)=1. In two dimensions, this is a square, in
%               three an octohedron, in n a cross polytope (or
%               n-dimensional octahedron).
%
%INPUTS: numDim An integer specifying the dimensionality of the points to
%               be generated.
%     algorithm A value indicating which algorithm should be used. Possible
%               values are:
%               0 (The default if omitted or an empty matrix is passed)
%                 Formula G_n 3-1 in [1], pg. 304, 2*numDim points.
%               1 Formula G_n 3-2 in [1], pg. 304, 2^numDim points,
%                 numDim<4.
%
%OUTPUTS: xi A numDim X numCubaturePoints matrix containing the cubature
%            points. (Each "point" is a vector)
%          w A numCubaturePoints X 1 vector of the weights associated with
%            the cubature points.
%
%REFERENCES:
%[1] A.H. Stroud, Approximate Calculation of Multiple Integrals. Cliffs,
%    NJ: Prentice-Hall, Inc., 1971.
%
%November 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release

if(nargin<2||isempty(algorithm))
   algorithm=0;
end

switch(algorithm)
    case 0%G_n 3-1 in [1], pg. 304, 2*numDim points.
        V=exp(numDim*log(2)-gammaln(numDim+1));
        n=numDim;

        r=sqrt(2*n/((n+1)*(n+2)));

        w=V/(2*n)*ones(2*n,1);
        xi=fullSymPerms([r;zeros(numDim-1,1)]);
    case 1%G_n 3-2 in [1], pg. 304, 2^numDim points, numDim<4.
        if(numDim>=4)
           error('This algorithm requires numDim<4')
        end

        V=exp(numDim*log(2)-gammaln(numDim+1));
        n=numDim;

        r=sqrt(2/((n+1)*(n+2)));

        w=2^(-n)*V*ones(2^n,1);
        xi=PMCombos(r*ones(numDim,1));
    otherwise
        error('Unknown algorithm specified');
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 area = polygon_area_2 ( n, v )

%*****************************************************************************80
%
%% POLYGON_AREA_2 computes the area of a polygon.
%
%  Discussion:
%
%    The area is the sum of the areas of the triangles formed by
%    node N with consecutive pairs of nodes.
%
%    If the vertices are given in counterclockwise order, the area
%    will be positive.  If the vertices are given in clockwise order,
%    the area will be negative.
%
%    Thanks to Martin Pineault for noticing that an earlier version
%    of this routine would not correctly compute the area of a nonconvex
%    polygon.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Adrian Bowyer, John Woodwark,
%    A Programmer's Geometry,
%    Butterworths, 1983.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%
%    Input, real V(2,N), the coordinates of the vertices.
%
%    Output, real AREA, the area of the polygon.
%
  area = 0.0;

  for i = 1 : n - 2

    area_triangle = triangle_area ( ...
      v(1,i),   v(2,i),   ...
      v(1,i+1), v(2,i+1), ...
      v(1,n),   v(2,n) );

    area = area + area_triangle;

  end

  return
end


function f = make_gaussian_filter(sigma, cutoff)
%MAKE_GAUSSIAN_FILTER   Create Gaussian filter
%   MAKE_GAUSSSIAN_FILTER(SIGMA[, CUTOFF]) creates a Gaussian filter with
%   standard deviation SIGMA.  The filter mask is cut off at CUTOFF times
%   the standard deviation.  If CUTOFF is omitted, a default value of 3
%   is assumed.
%
%   See also FSPECIAL.
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date$
%   $Revision$

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

  if (nargin < 2)
    cutoff = 3;
  end

  sz = ceil(cutoff * sigma);

  f = exp(-(-sz:sz).^2 / (2*sigma^2));
  f = f ./ sum(f);


function Z = projectData(X, U, K)
%PROJECTDATA Computes the reduced data representation when projecting only
%on to the top k eigenvectors
%   Z = projectData(X, U, K) computes the projection of
%   the normalized inputs X into the reduced dimensional space spanned by
%   the first K columns of U. It returns the projected examples in Z.
%

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

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the projection of the data using only the top K
%               eigenvectors in U (first K columns).
%               For the i-th example X(i,:), the projection on to the k-th
%               eigenvector is given as follows:
%                    x = X(i, :)';
%                    projection_k = x' * U(:, k);
%

% for i=1: size(X,1);
%   x = X(i, :)';
%   for k=1: K;
%     Z(i, k) = x' * U(:, k);
%   end
% end

U_reduce = U(:, 1:K);
Z = X * U_reduce;

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

end


function [ r, pc ] = tetrahedron_insphere_3d ( tetra )

%*****************************************************************************80
%
%% TETRAHEDRON_INSPHERE_3D finds the insphere of a tetrahedron in 3D.
%
%  Discussion:
%
%    The insphere of a tetrahedron is the inscribed sphere, which touches
%    each face of the tetrahedron at a single point.
%
%    The points of contact are the centroids of the triangular faces
%    of the tetrahedron.  Therefore, the point of contact for a face
%    can be computed as the average of the vertices of that face.
%
%    The sphere can then be determined as the unique sphere through
%    the four given centroids.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Schneider, David Eberly,
%    Geometric Tools for Computer Graphics,
%    Elsevier, 2002,
%    ISBN: 1558605940,
%    LC: T385.G6974.
%
%  Parameters:
%
%    Input, real TETRA(3,4), the vertices of the tetrahedron.
%
%    Output, real R, PC(3,1), the radius and the center
%    of the sphere.
%
  dim_num = 3;

  v21(1:dim_num,1) = tetra(1:dim_num,2) - tetra(1:dim_num,1);
  v31(1:dim_num,1) = tetra(1:dim_num,3) - tetra(1:dim_num,1);
  v41(1:dim_num,1) = tetra(1:dim_num,4) - tetra(1:dim_num,1);
  v32(1:dim_num,1) = tetra(1:dim_num,3) - tetra(1:dim_num,2);
  v42(1:dim_num,1) = tetra(1:dim_num,4) - tetra(1:dim_num,2);
  v43(1:dim_num,1) = tetra(1:dim_num,4) - tetra(1:dim_num,3);

  n123 = r8vec_cross_product_3d ( v21, v31 );
  n124 = r8vec_cross_product_3d ( v41, v21 );
  n134 = r8vec_cross_product_3d ( v31, v41 );
  n234 = r8vec_cross_product_3d ( v42, v32 );

  l123 = norm ( n123 );
  l124 = norm ( n124 );
  l134 = norm ( n134 );
  l234 = norm ( n234 );

  pc(1:dim_num,1) = ( l234 * tetra(1:dim_num,1)   ...
                    + l134 * tetra(1:dim_num,2)   ...
                    + l124 * tetra(1:dim_num,3)   ...
                    + l123 * tetra(1:dim_num,4) ) ...
                  / ( l234 + l134 + l124 + l123 );

  b(1:dim_num,1:4) = tetra(1:dim_num,1:4);
  b(4,1:4) = 1.0;

  gamma = abs ( r8mat_det_4d ( b ) );

  r = gamma / ( l234 + l134 + l124 + l123 );

  return
end


function cdf = log_series_cdf ( x, a )

%*****************************************************************************80
%
%% LOG_SERIES_CDF evaluates the Logarithmic Series CDF.
%
%  Discussion:
%
%    Simple summation is used, with a recursion to generate successive
%    values of the PDF.
%
%    Thanks to Oscar van Vlijmen.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 December 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X, the argument of the PDF.
%    0 < X
%
%    Input, real A, the parameter of the PDF.
%    0.0 < A < 1.0.
%
%    Output, real CDF, the value of the CDF.
%
  cdf = 0.0;

  for x2 = 1 : x

    if ( x2 == 1 )
      pdf = - a / log ( 1.0 - a );
    else
      pdf = ( x2 - 1 ) * a * pdf / x2;
    end

    cdf = cdf + pdf;

  end

  return
end


function [A,b,x] = shaw(n)
%SHAW Test problem: one-dimensional image restoration model.
%
% [A,b,x] = shaw(n)
%
% Discretization of a first kind Fredholm integral equation with
% [-pi/2,pi/2] as both integration intervals.  The kernel K and
% the solution f, which are given by
%    K(s,t) = (cos(s) + cos(t))*(sin(u)/u)^2
%    u = pi*(sin(s) + sin(t))
%    f(t) = a1*exp(-c1*(t - t1)^2) + a2*exp(-c2*(t - t2)^2) ,
% are discretized by simple quadrature to produce A and x.
% Then the discrete right-hand b side is produced as b = A*x.
%
% The order n must be even.

% Reference: C. B. Shaw, Jr., "Improvements of the resolution of
% an instrument by numerical solution of an integral equation",
% J. Math. Anal. Appl. 37 (1972), 83-112.

% Per Christian Hansen, IMM, 08/20/91.

% Check input.
if (rem(n,2)~=0), error('The order n must be even'), end

% Initialization.
h = pi/n; A = zeros(n,n);

% Compute the matrix A.
co = cos(-pi/2 + (.5:n-.5)*h);
psi = pi*sin(-pi/2 + (.5:n-.5)*h);
for i=1:n/2
  for j=i:n-i
    ss = psi(i) + psi(j);
    A(i,j) = ((co(i) + co(j))*sin(ss)/ss)^2;
    A(n-j+1,n-i+1) = A(i,j);
  end
  A(i,n-i+1) = (2*co(i))^2;
end
A = A + triu(A,1)'; A = A*h;

% Compute the vectors x and b.
a1 = 2; c1 = 6; t1 =  .8;
a2 = 1; c2 = 2; t2 = -.5;
if (nargout>1)
  x =   a1*exp(-c1*(-pi/2 + (.5:n-.5)'*h - t1).^2) ...
      + a2*exp(-c2*(-pi/2 + (.5:n-.5)'*h - t2).^2);
  b = A*x;
end


% QP test problem. Demo file
%
clc;

disp('----------------------------------');
disp('Problem #1:');
disp(' ');
disp('min x1^2 + x^2  - 3*x1 - 1/4*x2');
disp(' ');
disp('subject to');
disp(' |x1|+|x2| <= 1');
disp('----------------------------------');

H=2*eye(2);
q=[-3,-1/4]';
A=[1 1;1 -1;-1 1;-1 -1];
b=ones(4,1);

[x, obj, lambda, info] = qpng (H, q, A, b)

pause;
clc

disp('----------------------------------');
disp('Problem #2:');
disp(' ');
disp('min 4*x1^2 + 1 x1*x2 + 4*x2^2 + 3*x1 ? 4*x2');
disp(' ');
disp('s.t. x1+x2 <= 5');
disp('     x1-x2 == 0');
disp('     x1 >= 0');
disp('     x2 >= 0');
disp('----------------------------------');

H=[8 1;1 8];
q=[3 -4]';
A=[1 1; 1 -1];
b=[5 0]';
ctype=char(['U', 'E']');
lb=[0 0]';
ub=[];

[x, obj, lambda, info] = qpng (H, q, A, b,ctype,lb,ub)

pause;
clc

disp('----------------------------------');
disp('Problem #3:');
disp(' ');
disp('min .5*x1''*H*x + q''*x');
disp(' ');
disp('s.t. -2.5 <= x1 <= 2.5');
disp('----------------------------------');

H=[.16 -1.2 2.4 -1.4;
   -1.2 12.0 -27.0 16.8;
   2.4 -27.0 64.8 -42.0;
   -1.4 16.8 -42.0 28.0];
q=[5.04 -59.4 146.4 -96.6]';
A=[-1 0 0 0; 1 0 0 0];
b=[2.5; 2.5];

[x, obj, lambda, info] = qpng (H, q, A, b)

pause;
clc

disp('----------------------------------');
disp('Problem #4:');
disp(' Solve a portfolio example. ');
disp(' See Section 13.7 of "Applications of optimization with Xpress-MP.');
disp('----------------------------------');

H=[4 3 -1 0;
   3 6 1 0;
   -1 1 10 0;
   0 0 0 10];
q=[];
A=[1 1 1 1;
   8 9 12 7];
b=[1; 7];
ctype=char(['E', 'L']');
lb=zeros(4,1);
ub=[];

[x, obj, lambda, info] = qpng (H, q, A, b, ctype, lb, ub)

pause;
clc

disp('----------------------------------');
disp('Problem #5:');
disp(' An indefinite QP problem is not solved x*=(3, 25/4). ');
disp(' ');
disp('min .5*x1^2 - .5*x2^2');
disp(' ');
disp('s.t. x2 >= 0');
disp('s.t. x1+x2 >= 2');
disp('s.t. -5x1+4x2 <= 10');
disp('s.t. x1 <= 3');
disp('----------------------------------');

H=[1 0;0 -1];
q=[];
A=[0 1;1 2;-5 4;1 0];
b=[0 2 10 3]';
ctype=char(['L', 'L', 'U', 'U']);

[x, obj, lambda, info]=qpng(H, q, A, b)


function [r, c] = partial_corr_coef(S, i, j, Y)
% PARTIAL_CORR_COEF Compute a partial correlation coefficient
% [r, c] = partial_corr_coef(S, i, j, Y)
%
% S is the covariance (or correlation) matrix for X, Y, Z
% where X=[i j], Y is conditioned on, and Z is marginalized out.
% Let S2 = Cov[X | Y] be the partial covariance matrix.
% Then c = S2(i,j) and r = c / sqrt( S2(i,i) * S2(j,j) )
%

% Example: Anderson (1984) p129
% S = [1.0 0.8 -0.4;
%     0.8 1.0 -0.56;
%     -0.4 -0.56 1.0];
% r(1,3 | 2) = 0.0966
%
% Example: Van de Geer (1971) p111
%S = [1     0.453 0.322;
%     0.453 1.0   0.596;
%     0.322 0.596 1];
% r(2,3 | 1) = 0.533

X = [i j];
i2 = 1; % find_equiv_posns(i, X);
j2 = 2; % find_equiv_posns(j, X);
S2 = S(X,X) - S(X,Y)*inv(S(Y,Y))*S(Y,X);
c = S2(i2,j2);
r = c / sqrt(S2(i2,i2) * S2(j2,j2));


function [x,gpd,nu]=gdpower(N,k,c)
%GDPOWER Signal with power-law group delay.
%	[X,GPD,F]=GDPOWER(N,K,C) generates a signal with a
%	power-law group delay of the form tx(f) = T0 + C*f^(K-1).
%	The output signal is of unit energy.
%
%	N   : number of points in time		(must be even)
%	K   : degree of the power-law		(default : 0)
%	C   : rate-coefficient of the power-law group delay.
%	      C must be non-zero. 		(default : 1)
%	X   : time row vector containing the modulated signal samples
%	GPD : group delay	      (length : round(N/2)-1)
%	F   : frequency bins
%
%	Examples :
%	 sig=gdpower(64,0); tfrbert(sig,1:64,0.01,0.32,256,1);
%	 [sig,gpd,f]=gdpower(256,1/2); plot(gpd,f);
%
%	See also FMPOWER.

%	O. Lemoine - April, July 1996
%	Copyright (c) 1996 by CNRS (France).
%
%  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 should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin < 1),
 error ( 'At least one parameter.' );
elseif nargin==1,
 k=0; c=1;
elseif nargin==2,
 c=1;
end

if (N <= 0),
 error ('The signal length N must be strictly positive' );
end

t0=0;

Lnu=round(N/2);
nu=linspace(0,0.5,Lnu+1); nu=nu(2:Lnu+1);
AM=nu.^((k-2)/6);

if c==0,
 error('C must be non-zero');
end

t=1:N;
TFx=zeros(1,N);

if (k<1 & k~=0),
 d=N^k*c; t0=N/10;
 TFx(1:Lnu)=exp(-j*2*pi*(t0*(nu)+d*(nu).^k/k)).*AM;
 x=ifft(TFx).';
elseif k==0,
 d=c; t0=N/10;
 TFx(1:Lnu)=exp(-j*2*pi*(t0*(nu)+d*log(nu))).*AM;
 x=ifft(TFx).';
elseif k==1,
 t0=N;
 x=anapulse(N,t0);
elseif (k>1),
 d=N*2^(k-1)*c;
 TFx(1:Lnu)=exp(-j*2*pi*(t0*(nu)+d*(nu).^k/k)).*AM;
 x=ifft(TFx).';
end

if k~=1,
 gpd=t0+abs(sign(c)-1)/2*(N+1)+d*nu.^(k-1);
else
 gpd=t0*ones(1,N/2);
end

x=x-mean(x);
x=x/norm(x);


function loc=LMSloc(X)
%Syntax: loc=LMSloc(X)
%_____________________
%
% Calculates the Least Median of Squares (LMS) location parameter
% of the columns of a matrix X. If X is a vector, it returns the LMS
% location parameter of its components. If X is a scalar, it returns
% X.
%
% loc is the LMS estimated vector of locations.
% X is the matrix with the data sets.
%
% Reference:
% Rousseeuw PJ, Leroy AM (1987): Robust regression and outlier detection. Wiley.
%
%
% Alexandros Leontitsis
% Institute of Mathematics and Statistics
% University of Kent at Canterbury
% Canterbury
% Kent, CT2 7NF
% U.K.
%
% University e-mail: al10@ukc.ac.uk (until December 2002)
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% Sep 3, 2001.

if nargin<1 | isempty(X)==1
   error('Not enough input arguments.');
else
   % X must be 2-dimensional
   if ndims(X)>2
      error('Invalid data set.');
   end
   % If X is a row vector make it a column vector
   if size(X,1)==1
      X=X';
   end

   % n is the length of the data set
   n=size(X,1);
end

% For a single data point there is no need to proceed
if n==1
   loc=X;
else
   % Sort the data set to ascending order
   X=sort(X);

   % P. 169: the length of the "half" of the data points
   h=floor(n/2)+1;

   % P. 169: determine the shortest half and compute its midpoint
   j=1:n-h+1;
   range=X(j+h-1,:)-X(j,:);

   % Calculate the location parameter for every column of X
   for i=1:size(X,2);
      f=find(range(:,i)==min(range(:,i)));
      loc(i)=median((X(f+h-1,i)+X(f,i))/2);
   end
end


function z = g_three ( x, y )

%*****************************************************************************80
%
%% G_THREE evaluates a function which has 6 minima, 2 global, in [-3,3]x[-2,2].
%
%  Discussion:
%
%    Actually, F_THREE evaluates that function; this function returns the
%    log of that function plus 2, which is useful when trying to draw contours.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the arguments.
%
%    Output, real Z, the function value.
%
  z = ( 4.0 - 2.1 * x.^2 + x.^4 / 3.0 ) .* x.^2 ...
    + x .* y + ( - 4.0 + 4.0 * y.^2 ) .* y.^2;

  z = log ( 2 + z );

  return
end


function z = log_sigmoid(x)
% computes and returns the logistic sigmoid of x. Operation is performed
% element-wise if x is a vector or matrix.
%
%       z = 1 ./ (1-exp(-x))

z = (1+exp(-x)).^-1;

end


function v = r8vec_house_column ( n, a_vec, k )

%*****************************************************************************80
%
%% R8VEC_HOUSE_COLUMN defines a Householder premultiplier that "packs" a column.
%
%  Discussion:
%
%    The routine returns a vector V that defines a Householder
%    premultiplier matrix H(V) that zeros out the subdiagonal entries of
%    column K of the matrix A.
%
%       H(V) = I - 2 * v * v'
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix A.
%
%    Input, real A_VEC(N,1), column K of the matrix A.
%
%    Input, integer K, the column of the matrix to be modified.
%
%    Output, real V(N,1), a vector of unit L2 norm which defines an
%    orthogonal Householder premultiplier matrix H with the property
%    that the K-th column of H*A is zero below the diagonal.
%

%
%  Destroy all row vectors!
%
  a_vec = a_vec(:);

  v(1:n,1) = 0.0;

  if ( k < 1 || n <= k )
    return
  end

  s = sqrt ( sum ( a_vec(k:n,1).^2 ) );

  if ( s == 0.0 )
    return
  end

  if ( a_vec(k,1) < 0.0 )
    v(k,1) = a_vec(k,1) - abs ( s ) ;
  else
    v(k,1) = a_vec(k,1) + abs ( s );
  end
  v(k+1:n,1) = a_vec(k+1:n,1);
%
%  Normalize V.
%
  s = sqrt ( sum ( v(k:n,1).^2 ) );

  v(k:n,1) = v(k:n,1) / s;

  return
end


function [ q_min, q_max, q_ave, q_area ] = q_measure ( n, z, triangle_order, ...
  triangle_num, triangle_node )

%*****************************************************************************80
%
%% Q_MEASURE determines the triangulated pointset quality measure Q.
%
%  Discussion:
%
%    The Q measure evaluates the uniformity of the shapes of the triangles
%    defined by a triangulated pointset.
%
%    For a single triangle T, the value of Q(T) is defined as follows:
%
%      TAU_IN = radius of the inscribed circle,
%      TAU_OUT = radius of the circumscribed circle,
%
%      Q(T) = 2 * TAU_IN / TAU_OUT
%        = ( B + C - A ) * ( C + A - B ) * ( A + B - C ) / ( A * B * C )
%
%    where A, B and C are the lengths of the sides of the triangle T.
%
%    The Q measure computes the value of Q(T) for every triangle T in the
%    triangulation, and then computes the standard deviation of this
%    set of values:
%
%      Q_MEASURE = min ( all T in triangulation ) Q(T)
%
%    In an ideally regular mesh, all triangles would have the same
%    equilateral shape, for which Q = 1.  In a good mesh, 0.5 < Q.
%
%    Given the 2D coordinates of a set of N nodes, stored as Z(1:2,1:N),
%    a triangulation is a list of NT triples of node indices that form
%    triangles.  Generally, a maximal triangulation is expected, namely,
%    a triangulation whose image is a planar graph, but for which the
%    addition of any new triangle would mean the graph was no longer planar.
%    A Delaunay triangulation is a maximal triangulation which maximizes
%    the minimum angle that occurs in any triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, pages 329-345, June 2004.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real Z(DIM_NUM,N), the points.
%
%    Input, integer TRIANGLE_ORDER, the order of the triangles.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(TRIANGLE_ORDER,TRIANGLE_NUM), the triangulation.
%
%    Output, real ( kind = 8 ) Q_MIN, Q_MAX, the minimum and maximum values
%    of Q over all triangles.
%
%    Output, real ( kind = 8 ) Q_AVE, the average value of Q.
%
%    Output, real ( kind = 8 ) Q_AREA, the average value of Q, weighted by
%    the area of each triangle.
%
  q_min = r8_huge ( );
  q_max = - r8_huge ( );
  q_ave = 0.0;
  q_area = 0.0;
  area_total = 0.0;

  for triangle = 1 : triangle_num

    a_index = triangle_node(1,triangle);
    b_index = triangle_node(2,triangle);
    c_index = triangle_node(3,triangle);

    ab_length = sqrt ( ...
        ( z(1,a_index) - z(1,b_index) )^2 ...
      + ( z(2,a_index) - z(2,b_index) )^2 );

    bc_length = sqrt ( ...
        ( z(1,b_index) - z(1,c_index) )^2 ...
      + ( z(2,b_index) - z(2,c_index) )^2 );

    ca_length = sqrt ( ...
        ( z(1,c_index) - z(1,a_index) )^2 ...
      + ( z(2,c_index) - z(2,a_index) )^2 );

    q = ( bc_length + ca_length - ab_length ) ...
      * ( ca_length + ab_length - bc_length ) ...
      * ( ab_length + bc_length - ca_length ) ...
      / ( ab_length * bc_length * ca_length );

    x1 = z(1,triangle_node(1,triangle));
    y1 = z(2,triangle_node(1,triangle));
    x2 = z(1,triangle_node(2,triangle));
    y2 = z(2,triangle_node(2,triangle));
    x3 = z(1,triangle_node(3,triangle));
    y3 = z(2,triangle_node(3,triangle));

    area = 0.5 * abs ( x1 * ( y2 - y3 ) ...
                     + x2 * ( y3 - y1 ) ...
                     + x3 * ( y1 - y2 ) );

    q_min = min ( q_min, q );
    q_max = max ( q_max, q );
    q_ave = q_ave + q;
    q_area = q_area + q * area;

    area_total = area_total + area;

  end

  q_ave = q_ave / triangle_num;

  if ( 0.0 < area_total )
    q_area = q_area / area_total;
  else
    q_area = 0.0;
  end

  return
end


function fx = p18_fun ( n, x )

%*****************************************************************************80
%
%% P18_FUN evaluates the integrand for problem 18.
%
%  Integral:
%
%    Integral ( 0 <= x < +oo ) x^2 * exp ( - x / 2^beta ) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Piessens, Elise de Doncker-Kapenga,
%    Christian Ueberhuber, David Kahaner,
%    QUADPACK: A Subroutine Package for Automatic Integration,
%    Springer, 1983, page 84.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the point at which the integrand
%    is to be evaluated.
%
%    Output, real FX(N), the value of the integrand at X.
%
  beta = 1.0;

  fx(1:n) = x(1:n).^2 .* exp ( - x(1:n) / 2^beta );

  return
end


function ex4bvp
%EX4BVP  Example 4 of the BVP tutorial.
%   This is a nerve impulse model considered in Example 7.1 of
%   R. Seydel, From Equilibrium to Chaos, Elsevier, 1988.  The
%   differential equations
%
%      y1' = 3*(y1 + y2 - 1/3*y1^3 + lambda)
%      y2' = -(y1 - 0.7 + 0.8*y2)/3
%
%   are to be solved subject to periodic boundary conditions
%
%      y1(0) = y1(T)
%      y2(0) = y2(T)
%
%   This is an example of non-separated boundary conditions, meaning
%   that some conditions involve values of the solution at both ends
%   of the interval.  Periodic boundary conditions are a common source
%   of such problems.  BVP4C is unusual in that it accepts problems
%   with non-separated boundary conditions.
%
%   The parameter lambda has the value -1.3 in the text cited.  The
%   period T is unknown, which is to say that the length of the
%   interval is not known. Such problems require some preparation.
%   By scaling the independent variable to tau = t/T the problem is
%   posed on the fixed interval [0,1]. When this is done, T becomes
%   an unknown parameter, but BVP4C provides for unknown parameters.

% Copyright 2002, The MathWorks, Inc.

%  Periodic functions evaluated in EX4INIT are used as a guess for the
%  solution on a crude mesh of 5 equally spaced points.  A guess of 2*pi
%  is provided for the unknown parameter T.
solinit = bvpinit(linspace(0,1,5),@ex4init,2*pi);

options = bvpset('stats','on');

sol = bvp4c(@ex4ode,@ex4bc,solinit,options);
T = sol.parameters;

%  The independent variable needs to be rescaled to its original value
%  before plotting the solution.  The initial guess is also plotted.
figure
plot(T*sol.x,sol.y(1,:),T*solinit.x,solinit.y(1,:),'o')
legend('solution found','initial guess');
axis([0 T -2.2 2]);
title('Nerve impulse model');
ylabel('solution y_1(t)');
xlabel(['Periodic solution with period ',num2str(sol.parameters,4)]);

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

function v = ex4init(x)
%EX4INIT  Guess function for Example 4 of the BVP tutorial.
%   V = EX4INIT(X) returns a column vector V that is a guess for y(x).
v = [ sin(2*pi*x)
      cos(2*pi*x)];

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

function dydt = ex4ode(t,y,T);
%EX4ODE  ODE funcion for Example 4 of the BVP tutorial.
dydt = [ 3*T*(y(1) + y(2) - 1/3*(y(1)^3) - 1.3);
         (-1/3)*T*(y(1) - 0.7 + 0.8*y(2)) ];

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

function res = ex4bc(ya,yb,T)
%EX4BC  Boundary conditions for Example 4 of the BVP tutorial.
res = [ya(1) - yb(1)
       ya(2) - yb(2)
       T*(-1/3)*(ya(1) - 0.7 + 0.8*ya(2)) - 1];


% Euclidean projection on a rectangle
% Section 8.1.1, Boyd & Vandenberghe "Convex Optimization"
% Joelle Skaf - 10/07/05
%
% The projection of x0 on a rectangle C = {x | l <= x <= u} is given by
%           minimize || x - x0 ||^2
%               s.t.    l <= x <= u
% It is also given by P_C(x0)_k = l_k       if  x0_k <= l_k
%                                 x0_k      if  l_k <= x0_k <= u_k
%                                 u_k       if  x0_k >= u_k

% Input data: generate vectors l and u such that l < 0 < u
n  = 10;
l  = -rand(n,1);
u  = rand(n,1);
x0 = randn(n,1);

% Analytical solution
fprintf(1,'Computing the analytical solution ...');
pc_x0 = x0;
pc_x0(find(x0<=l)) = l(find(x0<=l));
pc_x0(find(x0>=u)) = u(find(x0>=u));
fprintf(1,'Done! \n');

% Solution via QP
fprintf(1,'Computing the optimal solution by solving a QP ...');

cvx_begin quiet
    variable x(n)
    minimize ( norm(x-x0) )
    x <= u;
    x >= l;
cvx_end

fprintf(1,'Done! \n');

% Verification
disp('-----------------------------------------------------------------');
disp('Verifying that the analytical solution and the solution obtained via QP are equal: ');
[pc_x0 x]


function area = triangle_area ( vert1, vert2, vert3 )

%*****************************************************************************80
%
%% TRIANGLE_AREA returns the area of a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    26 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, real VERT1(2), VERT2(2), VERT3(2), the vertices of
%    the triangle.
%
%    Output, real AREA, the area of the triangle.
%
  area = 0.5 * ...
    ( ...
        ( vert2(1) - vert1(1) ) * ( vert3(2) - vert1(2) ) ...
      - ( vert3(1) - vert1(1) ) * ( vert2(2) - vert1(2) ) ...
    );

  return
end


function [ x, seed ] = circular_normal_sample ( a, b, seed )

%*****************************************************************************80
%
%% CIRCULAR_NORMAL_SAMPLE samples the Circular Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A(2), a parameter of the PDF, the mean value.
%
%    Input, real B, a parameter of the PDF, the standard deviation.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(2), a sample of the PDF.
%
  [ v1, seed ] = r8_uniform_01 ( seed );
  [ v2, seed ] = r8_uniform_01 ( seed );

  r = sqrt ( - 2.0 * log ( v1 ) );

  x(1) = a(1) + b * r * cos ( 2.0 * pi * v2 );
  x(2) = a(2) + b * r * sin ( 2.0 * pi * v2 );

  return
end


function f = bohach2 ( m, x )

%*****************************************************************************80
%
%% BOHACH2 evaluates the Bohachevsky function #2.
%
%  Discussion:
%
%    The minimizer is
%
%      X* = [ 0.0, 0.0 ]
%      F(X*) = 0.0
%
%    Suggested starting point:
%
%      X = [ 0.6, 1.3 ];
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, integer M, the number of variables.
%
%    Input, real X(M), the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  f =       x(1) * x(1) ...
    + 2.0 * x(2) * x(2) ...
    - 0.3 * cos ( 3.0 * pi * x(1) ) * cos ( 4.0 * pi * x(2) ) ...
    + 0.3;

  return
end


function [X,W]=simplexquad(varargin)

% simplexquad.m - Gaussian Quadrature for an n-dimensional simplex.
%
% Construct Gauss points and weights for a n-dimensional simplex
% domain with vertices specified by the n*(n-1) matrix vert. Where each
% row contains the coordinates (x1,...,xn) for a vertex. The order of
% the quadrature scheme in each dimension must be the same in this
% implementation.
%
% Sample Usage:
%
% [X,W]=simplexquad(n,vert); % Specify the vertices
% [X,W]=simplexquad(n,dim);  % Specify the dimension and use unit simplex
%
% X will be a matrix for which the jth column are the grid points in each
% coordinate xj.
%
% Note: The standard n-dimensional simplex has vertices specified
%       vert=eye(n+1,n).
%
% The first four simplexes are
%
% n | Domain
% --|------------
% 1 | Interval
% 2 | Triangle
% 3 | Tetrahedron
% 4 | Pentatope
%
% Written by: Greg von Winckel
% Contact: gregvw(at)math(dot)unm(dot)edu
% http://math.unm.edu/~gregvw

nargin=length(varargin);

if nargin~=2
    error('simplexquad takes two input arguments');
end

N=varargin{1};

if length(N)~=1
    error('First argument must be a scalar');
end

if N~=abs(round(N-1))+1;
    error('Number of Gauss points must be a natural number');
end

if length(varargin{2})==1  % Dimension specified
    n=varargin{2};

    if n~=abs(round(n-1))+1;
        error('Dimension must be a natural number');
    end

    m=n+1; vert=eye(m,n);
else                      % Vertices specified
    vert=varargin{2};
    [m,n]=size(vert);

    if m~=n+1
        error('The matrix of vertices must have n+1 rows and n columns');
    end
end

Nn=N^n;

if n==1 % The 1-D simplex is only an interval
    [q,w]=rquad(N,0); len=diff(vert);
    X=vert(1)+len*q;  W=abs(len)*w;

else % Find quadrature rules for higher dimensional domains

    for k=1:n
        [q{k},w{k}]=rquad(N,n-k);
    end

    [Q{1:n}]=ndgrid(q{:}); q=reshape(cat(n,Q{:}),Nn,n);
    [W{1:n}]=ndgrid(w{:}); w=reshape(cat(n,W{:}),Nn,n);

    map=eye(m); map(2:m,1)=-1; c=map*vert;
    W=abs(det(c(2:m,:)))*prod(w,2);

    qp=cumprod(q,2); e=ones(Nn,1);
    X=[e [(1-q(:,1:n-1)),e].*[e,qp(:,1:n-2),qp(:,n)]]*c;
end

function [x,w]=rquad(N,k)

k1=k+1; k2=k+2; n=1:N;  nnk=2*n+k;
A=[k/k2 repmat(k^2,1,N)./(nnk.*(nnk+2))];
n=2:N; nnk=nnk(n);
B1=4*k1/(k2*k2*(k+3)); nk=n+k; nnk2=nnk.*nnk;
B=4*(n.*nk).^2./(nnk2.*nnk2-nnk2);
ab=[A' [(2^k1)/k1; B1; B']]; s=sqrt(ab(2:N,2));
[V,X]=eig(diag(ab(1:N,1),0)+diag(s,-1)+diag(s,1));
[X,I]=sort(diag(X));
x=(X+1)/2; w=(1/2)^(k1)*ab(1,2)*V(1,I)'.^2;


function divdif_test05 ( )

%*****************************************************************************80
%
%% DIVDIF_TEST05 tests DIF_TO_R8POLY and DIF_SHIFT_ZERO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 October 2006
%
%  Author:
%
%    John Burkardt
%
  maxtab = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIVDIF_TEST05\n' );
  fprintf ( 1, '  DIF_TO_R8POLY converts a difference table to a\n' );
  fprintf ( 1, '  polynomial;\n' );
  fprintf ( 1, '  DIF_SHIFT_ZERO shifts a divided difference\n' );
  fprintf ( 1, '  table to all zero abscissas;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  These are equivalent operations!\n' );
  fprintf ( 1, '\n' );
%
%  Set XTAB, YTAB to X, F(X)
%
  ntab = 4;

  xtab1 = r8vec_indicator ( ntab );

  ytab1(1:ntab) = xtab1(1:ntab).^3 - 2.0 * xtab1(1:ntab).^2 ...
    + 3.0 * xtab1(1:ntab) - 4.0;

  xtab2 = r8vec_indicator ( ntab );

  ytab2(1:ntab) = xtab2(1:ntab).^3 - 2.0 * xtab2(1:ntab).^2 ...
    + 3.0 * xtab2(1:ntab) - 4.0;
%
%  Compute and display the finite difference table.
%
  diftab1 = data_to_dif_display ( ntab, xtab1, ytab1 );

  diftab2 = data_to_dif_display ( ntab, xtab2, ytab2 );
%
%  Examine corresponding polynomial.
%
  dif_print ( ntab, xtab1, diftab1, '  The divided difference polynomial:' );
%
%  Shift to zero.
%
  [ xtab1, diftab1 ] = dif_shift_zero ( ntab, xtab1, diftab1 );

  r8poly_print ( ntab, diftab1, '  Using DIF_SHIFT_ZERO' );
%
%  Shift to zero.
%
  c = dif_to_r8poly ( ntab, xtab2, diftab2 );

  r8poly_print ( ntab, c, '  Using DIF_TO_R8POLY' );

  return
end


function b = isPerpendicular(v1, v2, varargin)
%ISPERPENDICULAR Check orthogonality of two vectors
%
%   B = isPerpendicular(V1, V2)
%   where V1 and V2 are two 1-by-2 row arrays, returns 1 if the vectors are
%   perpendicular, and 0 otherwise.
%
%   Also works when V1 and V2 are two N-by-2 arrays with same number of
%   rows. In this case, return a N-by-1 array containing 1 at the positions
%   of perpendicular vectors.
%
%   Also works when one of V1 or V2 is 1-by-2 and the other one is a N-by-2
%   array. In this case the result has size N-by-1.
%
%   B = isPerpendicular(V1, V2, ACCURACY)
%   specifies accuracy of numerical tests, default is 1e-14.
%
%
%   Example
%   isPerpendicular([1 2 1], [2 4 2])
%   ans =
%       1
%
%   isPerpendicular([1 2 1], [1 3 2])
%   ans =
%       0
%
%   See also
%   vectors2d, isParallel, lines2d
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-04-25
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   2007-09-18 copy from isPerpendicular, adapt to any dimension, and add
%       psb to specify precision
%   2009-09-21 fix bug for array of 3 vectors
%   2011-01-20 replace repmat by ones-indexing (faster)
%   2017-08-31 use normalized vectors

% default accuracy
acc = 1e-14;
if ~isempty(varargin)
    acc = abs(varargin{1});
end

% normalize vectors
v1 = normalizeVector(v1);
v2 = normalizeVector(v2);

% adapt size of inputs
n1 = size(v1, 1);
n2 = size(v2, 1);
if n1 ~= n2
    if n1 == 1
        v1 = v1(ones(n2, 1), :);
    elseif n2==1
        v2 = v2(ones(n1, 1), :);
    else
        error('Inputs must either have same size, or one must be scalar');
    end
end

% performs test
b = abs(dot(v1, v2, 2)) < acc;


function traj = ScrewTrajectory(Xstart, Xend, Tf, N, method)
% *** CHAPTER 9: TRAJECTORY GENERATION ***
% Takes Xstart: The initial end-effector configuration,
%       Xend: The final end-effector configuration,
%       Tf: Total time of the motion in seconds from rest to rest,
%       N: The number of points N > 1 (Start and stop) in the discrete
%          representation of the trajectory,
%       method: The time-scaling method, where 3 indicates cubic
%               (third-order polynomial) time scaling and 5 indicates
%               quintic (fifth-order polynomial) time scaling.
% Returns traj: The discretized trajectory as a list of N matrices in SE(3)
%               separated in time by Tf/(N-1). The first in the list is
%               Xstart and the Nth is Xend .
% This function calculates a trajectory corresponding to the screw motion
% about a space screw axis.
% Example Input:
%
% clear; clc;
% Xstart = [[1 ,0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 1]; [0, 0, 0, 1]];
% Xend = [[0, 0, 1, 0.1]; [1, 0, 0, 0]; [0, 1, 0, 4.1]; [0, 0, 0, 1]];
% Tf = 5;
% N = 4;
% method = 3;
% traj = ScrewTrajectory(Xstart, Xend, Tf, N, method)
%
% Output:
% traj =
%    1.0000         0         0    1.0000
%         0    1.0000         0         0
%         0         0    1.0000    1.0000
%         0         0         0    1.0000
%
%    0.9041   -0.2504    0.3463    0.4410
%    0.3463    0.9041   -0.2504    0.5287
%   -0.2504    0.3463    0.9041    1.6007
%         0         0         0    1.0000
%
%    0.3463   -0.2504    0.9041   -0.1171
%    0.9041    0.3463   -0.2504    0.4727
%   -0.2504    0.9041    0.3463    3.2740
%         0         0         0    1.0000
%
%   -0.0000    0.0000    1.0000    0.1000
%    1.0000   -0.0000    0.0000   -0.0000
%    0.0000    1.0000   -0.0000    4.1000
%         0         0         0    1.0000

timegap = Tf / (N - 1);
traj = cell(1, N);
for i = 1: N
    if method == 3
        s = CubicTimeScaling(Tf, timegap * (i - 1));
    else
        s = QuinticTimeScaling(Tf, timegap * (i - 1));
    end
    traj{i} = Xstart * MatrixExp6(MatrixLog6(TransInv(Xstart) * Xend) * s);
end
end


function [log_rad,angle]=base(m,n)
% Compute radius and angle matrix base for a given size of m,n
% Input:
%       m:  number of rows
%       n:  number of cols
% Output:
%   log_rad:    radius matrix, store the distance between a pixel and
%               the center of matrix
%     angle:    angle matrix, store the angle of a pixel

ctrm=ceil(m/2);
ctrn=ceil(n/2);

% x span from -m/2 to m/2 with interval 2/m
[x,y] = meshgrid(( (1:m)-ctrm)/(m/2),( (1:n)-ctrn)/(n/2));

% radius matrix
rad = sqrt(x.^2 + y.^2);
rad(m/2,n/2) =  rad(m/2,n/2-1);
log_rad  = log2(rad);

% angle matrix
angle = atan2(y,x);


%................................................................

function stiffness=formStiffness2D(GDof,numberElements,...
    elementNodes,numberNodes,nodeCoordinates,D,thickness)

% compute stiffness matrix
% for plane stress Q4 elements

stiffness=zeros(GDof);

% 2 by 2 quadrature
[gaussWeights,gaussLocations]=gauss2d('2x2');

for e=1:numberElements
  numNodePerElement = length(elementNodes(e,:));
  numEDOF = 2*numNodePerElement;
  elementDof=zeros(1,numEDOF);
  for i = 1:numNodePerElement
      elementDof(2*i-1)=2*elementNodes(e,i)-1;
      elementDof(2*i)=2*elementNodes(e,i);
  end

  % cycle for Gauss point
  for q=1:size(gaussWeights,1)
    GaussPoint=gaussLocations(q,:);
    xi=GaussPoint(1);
    eta=GaussPoint(2);

% shape functions and derivatives
    [shapeFunction,naturalDerivatives]=shapeFunctionQ4(xi,eta);

% Jacobian matrix, inverse of Jacobian,
% derivatives w.r.t. x,y
    [Jacob,invJacobian,XYderivatives]=...
        Jacobian(nodeCoordinates(elementNodes(e,:),:),naturalDerivatives);

%  B matrix
    B=zeros(3,numEDOF);
    B(1,1:2:numEDOF)       = XYderivatives(:,1)';
    B(2,2:2:numEDOF)  = XYderivatives(:,2)';
    B(3,1:2:numEDOF)       = XYderivatives(:,2)';
    B(3,2:2:numEDOF)  = XYderivatives(:,1)';

% stiffness matrix
    stiffness(elementDof,elementDof)=...
        stiffness(elementDof,elementDof)+...
        B'*D*thickness*B*gaussWeights(q)*det(Jacob);
  end
end


%Geometry domain
L=15; H=1; B=1; A=H*B; I = B*H^3/12;
interval = nrbline ([0 0],[L 0]);
problem_data.geo_name = interval;

%Boundary conditions
%Left end: Sliding restraint + concentrated force
%Right end: Simple support + moment
problem_data.drchlt1_ends = [false true]; %deflections, homogeneous
problem_data.drchlt2_ends = [true false]; %rotation angles, homogeneous
problem_data.nmnn1_ends   = [true false]; %forces
problem_data.nmnn2_ends   = [false true]; %moments

M(1) = 0; M(2) = 10;
P = 3;
problem_data.M = M; %moment values
problem_data.P = P; %force value

f_c = 0.5;
problem_data.f = @(x) f_c * sin(pi*x/L); %distributed loading

%Physical parameters
E   = 210000;
problem_data.EI = @(x) E * I* ones (size (x));

% Discretization parameters
method_data.degree     = 3;     % Degree of the basis functions
method_data.regularity = 2;     % Regularity of the basis functions
method_data.nsub       = 20;     % Number of subdivisions
method_data.nquad      = 5;     % Points for the Gaussian quadrature rule

% Solving
[geometry, msh, space, w] = solve_BE_Beam (problem_data, method_data);

%Postprocessing
%Exact solution
c1 = (L*f_c+P*pi)/(pi*E*I);
c2 = -(L^2*f_c+L*P*pi+M(2)*pi)/(pi*E*I);
c3 = -f_c*L^3/(pi^3*E*I);
c4 = L^2*(2*L^2*pi^2*f_c+2*L*P*pi^3+3*M(2)*pi^3+6*L^2*f_c)/(pi^3*E*I*6);

w_ex = @(x) f_c*sin(pi*x/L)/E/I*(L/pi)^4 + c1*x.^3/6 + c2*x.^2/2 + c3*x + c4;
%Gradient of exact solution
gradwex = @(x) f_c*cos(pi*x/L)/E/I*(L/pi)^3 + c1*x.^2/2 + c2*x + c3;
%Hessian of exact solution
hesswex = @(x) -f_c*sin(pi*x/L)/E/I*(L/pi)^2 + c1*x + c2;

%L2, H1 and H2 norms of error

[errh2, errh1, errl2] = sp_h2_error (space, msh, w, w_ex, gradwex, hesswex);
errors = [errh2, errh1, errl2]
[normh2, normh1, norml2] = sp_h2_error (space, msh, zeros (size (w)), w_ex, gradwex, hesswex);
relative_errors = [errh2/normh2, errh1/normh1, errl2/norml2]

[eu, F] = sp_eval (w, space, geometry, 100);
han = figure;
plot(F,eu,F,w_ex(F),'--')
legend('FEM-solution', 'Exact solution')
xlabel('x')
ylabel('w')

%!test
%! ex_BE_beam_static
%! close (han)
%! assert (errors, [1.65638054386650e-06 1.95594407043227e-07 4.14113201313776e-08], 2e-11);
%! assert (relative_errors, [1.31307379889248e-06 1.55064145756187e-07 3.30097917469054e-08], 2e-11);


function [energy, kinetic, potential] = energy_3_link(z,P)
%[ENERGY, KINETIC, POTENTIAL] = ENERGY_3_LINK(Z,P)
%
%FUNCTION:  This function computes the energy of a double
%    pendulum.
%INPUTS:
%
%
%OUTPUTS:
%    energy = [1 X nTime] vector of total energy
%    kinetic = [1 X nTime] vector of kinetic energy
%    potential = [1 X nTime] vector of potential energy
%
%NOTES:
%    This file was automatically generated by writeEnergy.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);

dth1 = z(4,:);
dth2 = z(5,:);
dth3 = z(6,:);

kinetic = (I1.*dth1.^2)./2 + (I2.*dth2.^2)./2 + (I3.*dth3.^2)./2 + (m1.*(d1.^2.*dth1.^2.*cos(th1).^2 + d1.^2.*dth1.^2.*sin(th1).^2))./2 + (m2.*((dth1.*l1.*cos(th1) + d2.*dth2.*cos(th2)).^2 + (d2.*dth2.*sin(th2) + dth1.*l1.*sin(th1)).^2))./2 + (m3.*((dth1.*l1.*cos(th1) + dth2.*l2.*cos(th2) + d3.*dth3.*cos(th3)).^2 + (d3.*dth3.*sin(th3) + dth1.*l1.*sin(th1) + dth2.*l2.*sin(th2)).^2))./2;
potential = g.*m3.*(d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2)) + g.*m2.*(d2.*sin(th2) + l1.*sin(th1)) + d1.*g.*m1.*sin(th1);
energy = potential + kinetic;

end