function [x, err] = rombergQuadrature(fun,tSpan,tol)
% [x, err] = rombergQuadrature(fun,tSpan,tol)
%
% Compute the integral(fun), over the domain tSpan, to an accuracy of tol,
% using Romberg quadrature. Fully vectorized.
%
% Good for high-accuracy quadrature over smooth vector functions.
%
% If necessary, this function will automatically sub-divide the interval to
% achieve the desired accuracy. This should only occur when fun is stiff or
% non-smooth.
%
% INPUTS:
%   fun = vector function to be integrated
%       dx = fun(t)
%           t = [1, nt] = time vector
%           dx = [nx, nt] = function value at each point in t
%   tSpan = [tLow, tUpp] = time span (domain) for integration
%   tol = [nx,1] = desired error tolerance along each dimension
%
% OUTPUT:
%   x = [nx,1] = integral along each dimension
%   err = [nx, 1] = error estimate along each dimension
%
% NOTES:
%   algorithm from:
%   http://www.math.usm.edu/lambers/mat460/fall09/lecture29.pdf
%

a = tSpan(1);
b = tSpan(2);
h = b-a;
f0 = fun(tSpan);
nx = size(f0,1);

if isscalar(tol)
    tol = tol*ones(nx,1);
end

nMin = 4;   % Complete at least this many iterations
nMax = 12;  % Maximum iteration count
nSubSegment = 4;  % Sub-divide segment if max iteration is reached

T = zeros(nMax,nMax,nx);  % (iteration, extrapolation, dimension)
Err = zeros(nMax,nx); %(extrapolation, dimension)

T(1,1,:) = 0.5*h*sum(f0,2);
h = h/2;

converged = false;
for j=2:nMax

    % Trapazoid method
    i = 1:2^(j-2);
    t = a+h*(2*i-1);
    f = fun(t);
    T(j,1,:) = 0.5*T(j-1,1,:) + reshape(h*sum(f,2),1,1,nx);

    % Richardson extrapolation
    for k=2:j
        T(j,k,:) = T(j,k-1,:) + ...
            ( 1/(4^(k-1)-1) )*( T(j,k-1,:) - T(j-1,k-1,:) );
    end
    h = h/2;

    % Check the error estimate for convergence
    Err(j,:) = T(j,k,:) - T(j-1,k-1,:);
    err = abs(Err(j,:))';
    if all( err < tol ) && j > nMin
        converged = true;
        break;
    end
end

x = reshape(T(j,j,:),nx,1);

if ~converged  %Then non-smooth problem. Recursively sub-divide interval.
    time = linspace(tSpan(1), tSpan(2), nSubSegment+1);
    x = zeros(nx,1);
    err = zeros(nx,1);
    for i=1:nSubSegment
        [xTmp, errTmp] = rombergQuadrature(fun,time([i, i+1]),tol);
        x = x + xTmp;
        err = max(err,errTmp);
    end
end

end


%GENTRUNK Generation of Trunk's classification problem of 2 Gaussian classes
%
%   A = GENTRUNK(N,K)
%
% INPUT
%   N       Dataset size, or 2-element array of class sizes (default: [50 50]).
%   K       Dimensionality of the dataset to be generated (default: 10).
%
% OUTPUT
%   A       Dataset.
%
% DESCRIPTION
% Generation of a K-dimensional 2-class dataset A of N objects. Both classes
% are Gaussian distributed with the idenity matrix as covariance matrix.
% The means of the first class are defined by ua(j) = 1/sqrt(j). The means
% for the second class are ub = -ua. These means are such that the Nearest
% Mean Classifier always shows peaking for a finite training set.
%
% REFERENCES
% 1. G.V. Trunk, A Problem of Dimensionality: A Simple Example, IEEE Trans.
% Pattern Analysis and Machine Intelligence, vol. 1, pp. 306-307, 1979
% 2. A.K. Jain, R.P.W. Duin, and J. Mao, Statistical Pattern Recognition:
% A Review, IEEE Transactions on Pattern Analysis and Machine Intelligence,
% vol. 22, pp. 4-37, 2000.
%
% EXAMPLE
% a = gentrunk([1000 1000],200);
% e = clevalf(a,nmc,[1:9 10:5:25 50:25:200],[5 5],25);
% plote(e)
%
% SEE ALSO (PRTools Guide)
% DATASETS, PRDATASETS

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: gentrunk.m,v 1.2 2009/01/27 13:01:42 duin Exp $

function A = gendats (N,k)

		if (nargin < 1), N = [50 50]; end
	if (nargin < 2), k = 10;	end

	% Set equal priors and generate random class sizes according to these.
	p = [0.5 0.5]; N = genclass(N,p);

	% Unit covariance matrices
	% GA = eye(k); GB = eye(k);
  % Trunk means
  ma = 1./sqrt(1:k);
  mb = -ma;

  A = prdataset([randn(N(1),k) + repmat(ma,N(1),1); randn(N(2),k) + repmat(mb,N(2),1)]);
  A = prdataset(A,genlab(N));
  A = setprior(A,p);

% 	U = prdataset([ma;mb],[1 2]');
% 	U = setprior(U,p);
%
% 	% Create dataset.
% 	A = gendatgauss(N,U,cat(3,GA,GB));
	A = setname(A,'Trunk''s Problem');

return


function geometry_test178 ( )

%*****************************************************************************80
%
%% TEST178 tests ROTATION_MAT2QUAT_3D, ROTATION_QUAT2MAT_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;

  a = [ ...
    0.43301269, -0.5,        0.75; ...
    0.25,        0.86602539, 0.43301269; ...
   -0.86602539,  0.0,        0.5 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST178\n' );
  fprintf ( 1, '  ROTATION_MAT2QUAT_3D computes a rotation\n' );
  fprintf ( 1, '    quaternion from a rotation matrix.\n' );
  fprintf ( 1, '  ROTATION_QUAT2MAT_3D computes a rotation matrix\n' );
  fprintf ( 1, '    from a rotation quaternion.\n' );

  r8mat_print ( dim_num, dim_num, a, '  The rotation matrix:' );

  q = rotation_mat2quat_3d ( a );

  r8vec_print ( 4, q, '  The rotation quaternion:' );

  a = rotation_quat2mat_3d ( q );

  r8mat_print ( dim_num, dim_num, a, '  The rotation matrix:' );

  return
end


function d = mahalanobis(varargin)
%MAHALANOBIS Computes the Mahalanobis distance.
%   D = MAHALANOBIS(Y, X) computes the Mahalanobis distance between
%   each vector in Y to the mean (centroid) of the vectors in X, and
%   outputs the result in vector D, whose length is size(Y, 1).  The
%   vectors in X and Y are assumed to be organized as rows.  The
%   input data can be real or complex. The outputs are real
%   quantities.
%
%   D = MAHALANOBIS(Y, CX, MX) computes the Mahalanobis distance
%   between each vector in Y and the given mean vector, MX. The
%   results are output in vector D, whose length is size(Y, 1).  The
%   vectors in Y are assumed to be organized as the rows of this
%   array. The input data can be real or complex. The outputs are
%   real quantities. In addition to the mean vector MX, the
%   covariance matrix CX of a population of vectors X also must be
%   provided. Use function COVMATRIX (Section 11.5) to compute MX and
%   CX.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.6 $  $Date: 2005/01/18 13:44:47 $

% Reference: Acklam, P. J. [2002]. "MATLAB Array Manipulation Tips
% and Tricks." Available at
%     home.online.no/~pjacklam/matlab/doc/mtt/index.html
% or at
%     www.prenhall.com/gonzalezwoodseddins

param = varargin; % Keep in mind that param is a cell array.
Y = param{1};
ny = size(Y, 1); % Number of vectors in Y.

if length(param) == 2
   X = param{2};
   % Compute the mean vector and covariance matrix of the vectors
   % in X.
   [Cx, mx] = covmatrix(X);
elseif length(param) == 3 % Cov. matrix and mean vector provided.
   Cx = param{2};
   mx = param{3};
else
   error('Wrong number of inputs.')
end
mx = mx(:)'; % Make sure that mx is a row vector.

% Subtract the mean vector from each vector in Y.
Yc = Y - mx(ones(ny, 1), :);

% Compute the Mahalanobis distances.
d = real(sum(Yc/Cx.*conj(Yc), 2));


function [xy,scalFact]=ellips2Mercator(latLon,a,f)
%%ELLIPS2MERCATOR Convert from ellipsoidal (geodetic) latitude and
%               longitude into Mercator Easting and Northing. The Mercator
%               projection is useful, because lines of constant heading
%               (i.e. degrees East of North) are straight lines on the map
%               and the projection is conformal. However, the poles are
%               placed at +/-Inf. The Mercator projection is a type of
%               cylindrical projection.
%
%INPUTS: latLon A 2XN matrix of ellipsoidal [latitude;longitude] points
%               given in radians.
%             a The semi-major axis of the reference ellipsoid. If this
%               argument is omitted, the value in
%               Constants.WGS84SemiMajorAxis is used.
%             f The flattening factor of the reference ellipsoid. If this
%               argument is omitted, the value in Constants.WGS84Flattening
%               is used.
%
%OUTPUTS: xy A 2XN matrix of [Easting;Northing] points in the Mercator
%            projection. Easting is related to longitude and Northing is
%            related to latitude. The units are those of the semi-major
%            axis and are typically meters.
%   scalFact The scale factor (both parallel and meridian) at the given
%            points.
%
%The Mercator projection is discussed in detail in Chapter 7 of [1].
%Equations for the conversion along with a commonly used but very
%inaccurate spherical Mercator conversion (the "Web" Mercator map) are
%given in [2]. The "web" Mercator conversion can be obtained by setting
%f to zero.
%
%REFERENCES:
%[1] J. P. Snyder, "Map projections- a working manual," U.S. Geological
%    Survey, Tech. Rep. 1395, 1987.
%[2] Office of Geomatics, "National geospatial-intelligence agency
%   standardization document: Implementation practice: Web mercator map
%   projection," National Geospatial-Intelligence Agency, Tech. Rep.
%   NGA.SIG.0011 1.0.0 WEBMERC, 18 Feb. 2014. [Online]. Available:
%   http://earth-info.nga.mil/GandG/wgs84/web_mercator/(U)%20NGA_SIG_0011_1.0.0_WEBMERC.pdf
%
%August 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<3||isempty(f))
    f=Constants.WGS84Flattening;
end

e2=2*f-f^2;
%The first numerical eccentricity of the ellipsoid.
e=sqrt(e2);

sinPhi=sin(latLon(1,:));
cosPhi=cos(latLon(1,:));
lambda=latLon(2,:);

x=a*lambda;
y=a*(atanh(sinPhi)-e*atanh(e*sinPhi));

xy=[x;y];

scalFact=sqrt(1-e2*sinPhi.^2)./cosPhi;

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 S = SeparateScales(X,L)
% SeparatesScales: Separates an image into an (orthonormal)
%                                    series of disjoint scales.
%  Usage:
%    S =  SeparateScales(X,L);
%  Inputs:
%    X     n by n image
%    L     scale of the coarsest coefficients
%  Outputs: S data structure which contains the FT of X tapered at
%           dyadic scales.
%
% Description
%   For each s = 1, ..., J - L,
%   S{s} is  a structure which contains the DFT of X tapered with a
%   Meyer-style window which isolates frequencies near a dyadic
%   frequency subband
%
%   S{s}         m by m matrix with m = 2*2^(j+1)); matrix
%                entries are Fourier coefficients of Img at
%                -2^(j+1) <= k1,k2 < 2^(j+1) after windowing with a
%                bandpass Meyer window which isolates frequencies
%                in the range 2^(j-1) <= |k| <= 2^j.
%
%                j = L - 2 + s
% See Also
%   Adj_ Separatescales, Curvelet02Xform
%
% By Emmanuel Candes, 2003-2004

  n = size(X,1);
  n2 = n/2;
  J = log2(n);
  deg = 3;

  scale = (L-1):(J-1);
  nscales = length(scale);
  S = cell(1,nscales);  % Create data structure

  F = fft2_mid0(X)/sqrt(prod(size(X)));

  %      Partition at Coarse Level
  [ix,w] = CoarseMeyerWindow(L-1,deg);
  w = [0 reverse(w(2:length(w))) w];

  w2 = w'*w; % Build 2D-window

  l = 2^L;
  lx = (n2 - l + 1):(n2 + l);
  S{1} = F(lx,lx).*w2;

  %      Loop to Get Partition for  j = L, ..., J - 1;
  for j = L:(J-1),

    l = min(2^(j+1),n2);
    wlo = zeros(1,l); whi = wlo;

    [ixf, wf] = CoarseMeyerWindow(j-1,deg);
    wlo(ixf+1)  = wf;
    wlo = [0 reverse(wlo(2:length(wlo))) wlo];

    if j < J -1,
      [ixp, wp] = CoarseMeyerWindow(j,deg);
      whi(ixp+1) = wp;
      whi = [0 reverse(whi(2:length(whi))) whi];
    end

    if j == J - 1,
      whi = ones(1,2*l);
    end

    w2 = sqrt((whi'*whi).^2 - (wlo'*wlo).^2); % Build 2D-window

    lx = (n2 - l + 1):(n2 + l);
    S{j - L + 2} = F(lx,lx).*w2;
  end


function [w,b,Yt] = ridgeregress(X,Y,gam, Xt)
% Linear ridge regression
%
% >> [w, b]     = ridgeregress(X, Y, gam)
% >> [w, b, Yt] = ridgeregress(X, Y, gam, Xt)
%
% Ordinary Least squares with a regularization parameter (gam).
%
% Full syntax
%
% >> [w, b, Yt] = ridgeregress(X, Y, gam, Xt)
%
% Outputs
%   w     : d x 1 vector with the regression coefficients
%   b     : bias term
%   Yt(*) : Nt x 1 vector with predicted outputs of test data
% Inputs
%   X     : N x d matrix with the inputs of the training data
%   Y     : N x 1 vector with the outputs of the training data
%   gam   : Regularization parameter
%   Xt(*) : Nt x d matrix with the inputs of the test data
%
% See also:
% bay_rr,bay_lssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

if size(X,1)~=size(Y,1),
  error('X and Y need to have the same number of data points');
end
if size(Y,2)~=1,
  error('Only handling one-dimensional output');
end
if nargin==4 & size(Xt,2)~=size(X,2),
  error('Training input and test inputs need to have the same dimension');
end

[nD,nx] = size(X);
if nx>nD, warning('dim datapoints larger than number of datapoints...');end

Xe = [X ones(nD,1)];
%H = [ Xe'*Xe + gam^-1*[eye(nx) zeros(nx,1); zeros(1,nx+1)]];
H = Xe'*Xe + inv(gam).*eye(nx+1);

sol = pinv(H)*Xe'*Y;
w = sol(1:end-1);
b = sol(end);

if nargin<4, return; end
Yt = Xt*w+b;


function table = hen_power_product ( p, e )

%*****************************************************************************80
%
%% HEN_POWER_PRODUCT: power products, x^e*Hen(i,x)*Hen(j,x).
%
%  Discussion:
%
%    Hen(i,x) is the normalized probabilist's Hermite polynomial of degree I.
%
%    For polynomial chaos applications, it is of interest to know the
%    value of the integrals of products of X with every possible pair
%    of basis functions.  That is, we'd like to form
%
%      Tij = Integral ( -oo < X < +oo ) X^E * Hen(I,X) * Hen(J,X) exp(-0.5*X*X) dx
%
%    We will estimate these integrals using Gauss-Hermite quadrature.
%
%    When E is 0, the computed table should be the identity matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the maximum degree of the polyonomial factors.
%    0 <= P.
%
%    Input, integer E, the exponent of X in the integrand.
%    0 <= E.
%
%    Output, real TABLE(P+1,P+1), the table of integrals.  TABLE(I,J)
%    represents the weighted integral of X^E * Hen(I+1,X) * Hen(J+1,X).
%
  table(1:p+1,1:p+1) = 0.0;

  order = p + 1 + floor ( ( e + 1 ) / 2 );

  [ x_table, w_table ] = he_quadrature_rule ( order );

  for k = 1 : order

    x = x_table(k);
    h_table = hen_polynomial_value ( 1, p, x );
%
%  The following formula is an outer product in H_TABLE.
%
    if ( e == 0 )
      table(1:p+1,1:p+1) = table(1:p+1,1:p+1) ...
        + w_table(k) * h_table(1:p+1)' * h_table(1:p+1);
    else
      table(1:p+1,1:p+1) = table(1:p+1,1:p+1) ...
        + w_table(k) * x^e * h_table(1:p+1)' * h_table(1:p+1);
    end

  end

  return
end


function [ p, seed ] = cylinder_sample_3d ( p1, p2, r, n, seed )

%*****************************************************************************80
%
%% CYLINDER_SAMPLE_3D samples a cylinder in 3D.
%
%  Discussion:
%
%    We are sampling the interior of a right finite cylinder in 3D.
%
%    The interior of a (right) (finite) cylinder in 3D is defined by an axis,
%    which is the line segment from point P1 to P2, and a radius R.  The points
%    on or inside the cylinder are:
%    * points whose distance from the line through P1 and P2 is less than
%      or equal to R, and whose nearest point on the line through P1 and P2
%      lies (nonstrictly) between P1 and P2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(3,1), P2(3,1), the first and last points
%    on the axis line of the cylinder.
%
%    Input, real R, the radius of the cylinder.
%
%    Input, integer N, the number of sample points to compute.
%
%    Input, integer SEED, the random number seed.
%
%    Input, real P(3,N), the sample points.
%
%    Output, integer SEED, the random number seed.
%

%
%  Compute the axis vector.
%
  axis(1:3,1) = p2(1:3,1) - p1(1:3,1);
  axis_length = r8vec_norm ( 3, axis );
  axis(1:3,1) = axis(1:3,1) / axis_length;
%
%  Compute vectors V2 and V3 that form an orthogonal triple with AXIS.
%
  [ v2, v3 ] = plane_normal_basis_3d ( p1, axis );
%
%  Assemble the randomized information.
%
  [ radius(1,1:n), seed ] = r8vec_uniform_01 ( n, seed );
  radius(1,1:n) = r * sqrt ( radius(1,1:n) );

  [ theta(1,1:n), seed ] = r8vec_uniform_01 ( n, seed );
  theta(1,1:n) = 2.0 * pi * theta(1,1:n);

  [ z(1,1:n), seed ] = r8vec_uniform_01 ( n, seed );
  z(1,1:n) = axis_length * z(1,1:n);

  for i = 1 : 3

    p(i,1:n) = p1(i,1) ...
              + axis(i,1) * z(1,1:n) ...
              + v2(i,1) * radius(1,1:n) .* cos ( theta(1,1:n) ) ...
              + v3(i,1) * radius(1,1:n) .* sin ( theta(1,1:n) );

  end

  return
end


classdef ChiD
%%CHID Functions to handle the chi distribution.
%These distributions arise when taking the square root of the sum of the
%squares of independent normal random variables.
%Implemented methods are: mean, var, moments, PDF, CDF, (supports
%                         noncentral distributions) rand, entropy
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)

function val=mean(nu)
%%MEAN Obtain the mean of central chi probability distribution.
%
%INPUTS: nu The number of degrees of freedom of the central chi
%           distribution. Note that nu>0.
%
%OUTPUTS: val The mean of the central chi distribution.
%
%The mean of the central chi distribution is given in [1].
%
%REFERENCES:
%[1] Weisstein, Eric W. "Chi Distribution." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/ChiDistribution.html
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=sqrt(2)*exp(gammaln((nu+1)/2)-gammaln(nu/2));
end

function val=var(nu)
%%VAR Obtain the variance of the central chi probability distribution.
%
%INPUTS: nu The number of degrees of freedom of the chi distribution. Note
%           that nu>0.
%
%OUTPUTS: val  The variance of the central chi distribution.
%
%The variance of the central chi distribution is given in [1].
%
%REFERENCES:
%[1] Weisstein, Eric W. "Chi Distribution." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/ChiDistribution.html
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=2*exp(gammaln(1+nu/2)-gammaln(nu/2))-ChiD.mean(nu)^2;
end

function val=moments(m,nu)
%%MOMENTS Obtain noncentral moments of the chi distribution.
%
%INPUTS: m The order of the moment m>=0. The zeroth order moment is just 1.
%       nu The number of degrees of freedom of the chi distribution. Note
%          that nu>0.
%
%OUTPUTS: val  The mth-degree noncentral moments of the chi distribution.
%
%Moments of the chi distribution are given in [1].
%
%REFERENCES:
%[1] Weisstein, Eric W. "Chi Distribution." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/ChiDistribution.html
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=2^(m/2)*exp(gammaln((1/2)*(nu+m))-gammaln(nu/2));
end

function val=PDF(x,nu)
%%PDF Evaluate the central chi probability distribution function (PDF).
%
%INPUTS: x The point(s) at which the central chi PDF is to be evaluated.
%          Note that x>=0 for a nonzero probability.
%       nu The number of degrees of freedom of the chi distribution. Note
%          that nu>0. Numerical precision problems can arise for very large
%          values of nu.
%
%OUTPUTS: val The value(s) of the central chi PDF evaluated at x.
%
%The PDF is given in [1].
%
%REFERENCES:
%[1] Weisstein, Eric W. "Chi Distribution." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/ChiDistribution.html
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=zeros(size(x));
    sel=(x>=0);

    y=x(sel);
    val(sel)=2^(1-nu/2)*y.^(nu-1).*exp(-y.^2/2)/gamma(nu/2);
end

function prob=CDF(x,nu)
%%CDF Evaluate the cumulative distribution function (CDF) of the central
%     chi distribution at desired points.
%
%INPUTS: x The point(s) at which the chi CDF is to be evaluated.
%       nu The number of degrees of freedom of the chi-square
%          distribution. Note that nu>0.
%
%OUTPUTS: prob The value(s) of the CDF of the chi distribution with nu
%              degrees of freedom evaluated at x.
%
%The CDF of the central chi distribution is given in [1].
%
%REFERENCES:
%[1] Weisstein, Eric W. "Chi Distribution." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/ChiDistribution.html
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.

    prob=gammainc(x.^2/2,nu/2);
end

function vals=rand(N,nu,lambda)
%%RAND Generate chi distributed random variables with the given parameters.
%
%INPUTS: N If N is a scalar, then rand returns an NXN matrix of random
%          variables. If N=[M,N1] is a two-element row vector, then rand
%          returns an MXN1 matrix of random variables.
%       nu The number of degrees of freedom of the chi distribution. Note
%          that nu>0. If lambda is not zero, nu >=1.
%   lambda The non-centrality parameter of the distribution. In the central
%          chi distribution, this is zero. If this parameter is omitted or
%          an empty matrix is passed, then 0 is used.
%
%OUTPUTS: vals A matrix whose dimensions are determined by N of the
%              generated chi-squared random variables.
%
%A chi distributed random variable is just the square root of a chi-squared
%random variable. Thus, this function takes the square root of
%ChiSquareD.rand(N,nu,lambda).
%
%EXAMPLE:
%Here, we demonstrate that a histogram of many random samples will tend to
%agree with the PDF of the central distribution.
% nu=4.4;
% lambda=0;
% numSamp=[10000,1];
% samp=ChiD.rand(numSamp,nu);
% figure(1)
% clf
% hold on
% h=histogram(samp,'Normalization','pdf');
% %We will plot the PDF.
% numPoints=100;
% x=linspace(0,20,numPoints);
% PDFVals=ChiD.PDF(x,nu);
% plot(x,PDFVals,'-r','linewidth',2)
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(lambda))
        lambda=0;
    end

    if(isscalar(N))
        dims=[N N];
    else
        dims=N;
    end

    vals=sqrt(ChiSquareD.rand(dims,nu,lambda));
end

function entropyVal=entropy(nu)
%%ENTROPY Obtain the differential entropy of the central chi distribution
%         given in nats. The differential entropy of a continuous
%         distribution is entropy=-int_x p(x)*log(p(x))  dx where the
%         integral is over all values of x. Units of nats mean that the
%         natural logarithm is used in the definition. Unlike the Shannon
%         entropy for discrete variables, the differential entropy of
%         continuous variables can be both positive and negative.
%
%INPUTS: nu The number of degrees of freedom of the chi distribution. Note
%           that nu>0.
%
%OUTPUTS: entropyVal The value of the differential entropy in nats.
%
%Differential entropy is defined in Chapter 8 of [1].
%
%REFERENCES:
%[1] T. M. Cover and J. A. Thomas, Elements of Information Theory, 2nd ed.
%    Hoboken, NJ: Wiley-Interscience, 2006.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    entropyVal=gammaln(nu/2)+(1/2)*(nu-log(2)-(nu-1)*psi(nu/2));
end
end
end

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


function [xi,w]=thirdOrderSpherShellCubPoints(numDim,rMin)
%%THIRDORDERSPHERSHELLCUBPOINTS Obtain third order cubature points and
%               weight for integrating spherical region surrounding the
%               origin from radius rMin to radius 1, where rMin>0.
%
%INPUTS: numDim An integer specifying the dimensionality of the points to
%               be generated.
%          rMin The minimum radius of the spherical shell. 0<rMin<1.
%
%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.
%
%This implements formula S_n^{shell} 3-1 in [1], pg. 293, 2*numDim 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.

rho=rMin;
n=numDim;
V=(2*pi^(n/2)/(n*gamma(n/2)))*(1-rho^n);

r=sqrt(n/(n+2)*((1-rho^(n+2))/(1-rho^n)));
B=V/(2*n);

xi=fullSymPerms([r;zeros(n-1,1)]);
w=B*ones(2*n,1);

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 [aVal,aJacob,aHess,papt]=aCVPolar(x)
%%ACVPOLAR The drift function for a continuous-time motion model where the
%          target state is given in polar coordinates and the motion is
%          constant velocity in 2D Cartesian coordinates. This function
%          works regardless of whether the angle is measured
%          counterclockwise from the x axis or clockwise form the y axis.
%
%INPUTS: x The 4XN state vector of N targets in the order of
%          [r;theta;rDot;thetaDot]. The angle theta is given in radians.
%
%OUTPUTS: aVal The 4XN set of time-derivatives of the N state vectors under
%              the Cartesian linear motion model in polar coordinates.
%       aJacob This and higher partial derivatives can only be requested
%              if N=1. This is the 4X4  matrix of partial derivatives
%              of aVal such that aJacob(:,i) is the partial derivative of
%              aVal with respect to x(i).
%        aHess The 4X4X4  matrix of second derivatives of aVal such
%              that aHess(:,k1,k2) is the second partial derivative of
%              aVal with respect to x(k1) and x(k2).
%         papt The 4X1  partial derivative with resect to time of aVal.
%              This is all zeros, because the model is time invariant.
%
%A full derivation of the dynamic model is provided in [1]. Summarizing it
%here, let rVec be a position vector. We shall use the orthonormal basis
%vectors
%u_r=[cos(theta);sin(theta)] and u_theta=[-sin(theta);cos(theta)]
%if one is measuring theta from the x axis counter clockwise and
%u_r=[sin(theta);cos(theta)] and u_theta=[cos(theta);-sin(theta)]
%if one is measuring theta from the y axis clockwise.
%Note that u_theta is the derivative of u_r with respect to theta and the
%two vectors are orthonormal.
%A position can be expressed
%rVec=r*u_r
%Note the identities for the derivatives (dot terms):
%uDot_r=thetaDot*du_r/dtheta=dTheta*u_theta
%and uDot_theta=-thetaDor*u_r
%The velocity vector is
%rVecDot=rDot*u_r+r*uDot_r=rDot*u_r+r*thetaDot*u_theta
%Taking one more derivative to get an acceleration vectors, one has, after
%simplification:
%rDDot=(rDDot-r*thetaDot^2)*u_r+(r*thetaDDot+2*rDot*thetaDot)*u_theta
%where the coefficient of u_r is the radial acceleration term and the
%coefficient of u_theta is the angular acceleration term. Noting that for
%linear motion both accelerations must be zero, one gets the following two
%equations for the second derivative terms:
%rDDot=r*thetaDot^2
%thetaDDot=-(2/r)*rDot*thetaDot
%Thus, one has a linear dynamic model in 2D polar coordinates.
%
%EXAMPLE 1:
%Here, we verify that integrating forward with this model is equivalent to
%linear motion in Cartesian coordinates.
% xInitCart=[1000;40;-100;20];
% T=50;%The prediction time.
% F=FPolyKal(T,4,1);
% systemType=0;
% xInitSpher=stateCart2Pol(xInitCart,systemType);
% xEndCart=F*xInitCart;
% RelTol=1e-10;
% AbsTol=1e-13;
% xStepsPolar=RKAdaptiveOverRange(xInitSpher,[0;T],@(x,t)aCVPolar(x),0.1,0,[],[],RelTol,AbsTol);
% xEndPolarRK=xStepsPolar(:,end);
% xEndPolarExact=stateCart2Pol(xEndCart,systemType);
% max(abs(xEndPolarRK-xEndPolarExact))
%One will observe that the error is less than 1e-7, which is a good
%agreement.
%
%EXAMPLE 2:
%Here, we verify that the Jacobian is consistent with the numerical
%derivative of aDeriv.
% x=[1000;-0.1;10;0.01];
% [aDeriv,aJacob]=aCVPolar(x);
% AJacobNumDiff=numDiff(x,@(xState)aCVPolar(xState),4);
% err=(aJacob-AJacobNumDiff)./AJacobNumDiff;
% max(abs(err(:)))
%One will see that the maximum error is on the order of 1.6e-11, indicating
%good agreement.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic Linear Dynamic Models in Local Coordinates,"
%    Naval Research Laboratory, Washington, D.C., Tech. Rep.
%    NRL/MR/5344--19-9882, 24 Aug. 2019.
%
%August 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

r=x(1,:);
rDot=x(3,:);
thetaDot=x(4,:);

rDDot=r.*thetaDot.^2;
thetaDDot=-(2./r).*rDot.*thetaDot;

aVal=[rDot;thetaDot;rDDot;thetaDDot];

if(nargout>1)
    N=size(x,2);

    if(N>1)
        error('Derivatives are only available for numPoints=1.')
    end

    aJacob=[0                    0,  1,                 0;
            0,                   0,  0,                 1;
            thetaDot^2,          0,  0,                 2*r*thetaDot;
            2*rDot*thetaDot/r^2, 0,  -((2*thetaDot)/r), -((2*rDot)/r)];

    if(nargout>2)
        aHess=zeros(4,4,4);

        aHess(:,:,1)=[zeros(2,4);
                      0,                        0,  0,                2*thetaDot;
                      -((4*rDot*thetaDot)/r^3), 0,  (2*thetaDot)/r^2, (2*rDot)/r^2];
        aHess(:,:,3)=[zeros(3,4);
                      2*thetaDot/r^2,0,0,-(2/r)];
        aHess(:,:,4)=[zeros(2,4);
                      2*thetaDot, 0,  0,        2*r;
                      2*rDot/r^2,   0,  -((2)/r), -((2*rDot)/r)];

        if(nargout>3)
            papt=zeros(4,1);
        end
    end
end
end

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


% .............................................................
    function [shape,naturalDerivatives]=shapeFunctionQ4(xi,eta)

    % shape function and derivatives for Q4 elements
    % shape : Shape functions
    % naturalDerivatives: derivatives w.r.t. xi and eta
    % xi, eta: natural coordinates (-1 ... +1)

    shape=1/4*[ (1-xi)*(1-eta);
                (1+xi)*(1-eta);
                (1+xi)*(1+eta);
                (1-xi)*(1+eta)];
    naturalDerivatives=...
           1/4*[-(1-eta), -(1-xi);
                   1-eta, -(1+xi);
		           1+eta,    1+xi;
                -(1+eta),    1-xi];

    end % end function shapeFunctionQ4


function [z, mu, sigma] = VBA_zscore (x, flag, dim, omitnan)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [z, mu, sigma] = VBA_zscore (x, flag, dim, ignore_nans)
% standard score, aka z-score
%
% Compute the stardardized zscore of x, ie z = (x - mean(x)) / std(x)
%
% IN:
%   x: vector or matrix to normalize
%   flag: if true, normalizes x using std(X,1), i.e., by computing the
%     standard deviation(s) using N rather than N-1, where N is the length of
%     the dimension along which VBA_zscore works. VBA_zscore(x,0) is the same as
%     VBA_zscore(x). Default = false;
%   dim: dimension along which the standardization is performed. By
%     default, uses the first non singleton dimension.
%   ignore_nans: if true, use nan-safe normalization (default = false).
%
% OUT: z-score of x
%
% /////////////////////////////////////////////////////////////////////////

if nargin < 2 || isempty (flag)
	flag = 0;
else
    flag = +(flag > 0);
end

if nargin < 3 || isempty (dim)
	dim = find (size(x) > 1, 1);
	if isempty(dim)
		dim = 1;
	end
end

if nargin < 4
    omitnan = false;
end

if omitnan
    mu = VBA_nanmean (x, dim);
    sigma = sqrt(VBA_nanvar (x, dim, flag));
else
    mu = mean (x, dim);
    sigma = std (x, flag, dim);
end
sigma(sigma == 0) = 1;

z = bsxfun(@minus, x, mu);
z = bsxfun(@rdivide, z, sigma);


function variance = power_variance ( a, b )

%*****************************************************************************80
%
%% POWER_VARIANCE returns the variance of the Power PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < A, 0.0 < B.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = b * b * a / ( ( a + 1.0 )^2 * ( a + 2.0 ) );

  return
end


% PROBABILITY DISTRIBUTION FUNCTIONS (in the dist-folder):
%
% Priors
%  PRIOR_FIXED       Fix parameter to its current value
%  PRIOR_GAMMA       Gamma prior structure
%  PRIOR_GAUSSIAN    Gaussian prior structure
%  PRIOR_INVGAMMA    Inverse-gamma prior structure
%  PRIOR_INVT        Inverse Student-t prior structure
%  PRIOR_INVT        Inverse uniform prior structure
%  PRIOR_LAPLACE     Laplace (double exponential) prior structure
%  PRIOR_LOGGAUSSIAN Log-Gaussian prior structure
%  PRIOR_LOGLOGUNIF  Uniform prior structure for the log(log(parameter))
%  PRIOR_LOGT        Student-t prior structure for the logarithm of the parameter
%  PRIOR_LOGUNIF     Uniform prior structure for the logarithm of the parameter
%  PRIOR_SINVCHI2    Scaled inverse-chi-square prior structure
%  PRIOR_SQRTT       Student-t prior structure for the square root of the
%                    parameter
%  PRIOR_SQRTUNIF    Uniform prior structure for the square root of the
%                    parameter
%  PRIOR_UNIF        Uniform prior structure
%  PRIOR_T           Student-t prior structure
%  PRIOR_SQRTUNIF    Uniform prior structure for the square root of the
%                    parameter
%
% Probability density functions
%
%    BETA_LPDF     - Beta log-probability density function (lpdf).
%    BETA_PDF      - Beta probability density function (pdf).
%    DIR_LPDF      - Log probability density function of uniform Dirichlet
%                    distribution
%    DIR_PDF       - Probability density function of uniform Dirichlet
%                    distribution
%    GAM_CDF       - Cumulative of Gamma probability density function (cdf).
%    GAM_LPDF      - Log of Gamma probability density function (lpdf).
%    GAM_PDF       - Gamma probability density function (pdf).
%    GEO_LPDF      - Geometric log probability density function (lpdf).
%    INVGAM_LPDF   - Inverse-Gamma log probability density function.
%    INVGAM_PDF    - Inverse-Gamma probability density function.
%    LAPLACE_LPDF  - Laplace log-probability density function (lpdf).
%    LAPLACE_PDF   - Laplace probability density function (pdf).
%    LOGN_LPDF     - Log normal log-probability density function (lpdf)
%    LOGT_LPDF     - Log probability density function (lpdf) for log Student's T
%    MNORM_LPDF    - Multivariate-Normal log-probability density function (lpdf).
%    MNORM_PDF     - Multivariate-Normal log-probability density function (lpdf).
%    NORM_LPDF     - Normal log-probability density function (lpdf).
%    NORM_PDF      - Normal probability density function (pdf).
%    POISS_LPDF    - Poisson log-probability density function.
%    POISS_PDF     - Poisson probability density function.
%    SINVCHI2_LPDF - Scaled inverse-chi log-probability density function.
%    SINVCHI2_PDF  - Scaled inverse-chi probability density function.
%    T_LPDF        - Student's T log-probability density function (lpdf)
%    T_PDF         - Student's T probability density function (pdf)
%
% Random number generators
%
%    CATRAND       - Random matrices from categorical distribution.
%    DIRRAND       - Uniform dirichlet random vectors
%    EXPRAND       - Random matrices from exponential distribution.
%    GAMRAND       - Random matrices from gamma distribution.
%    INTRAND       - Random matrices from uniform integer distribution.
%    INVGAMRAND    - Random matrices from inverse gamma distribution
%    INVGAMRAND1   - Random matrices from inverse gamma distribution
%    INVWISHRND    - Random matrices from inverse Wishart distribution.
%    NORMLTRAND    - Random draws from a left-truncated normal
%                    distribution, with mean = mu, variance = sigma2
%    NORMRTRAND    - Random draws from a right-truncated normal
%                    distribution, with mean = mu, variance = sigma2
%    NORMTRAND     - Random draws from a normal truncated to interval
%    NORMTZRAND    - Random draws from a normal distribution truncated by zero
%    SINVCHI2RAND  - Random matrices from scaled inverse-chi distribution
%    TRAND         - Random numbers from Student's t-distribution
%    UNIFRAND      - Generate unifrom random numberm from interval [A,B]
%    WISHRND       - Random matrices from Wishart distribution.
%
% Others
%    KERNELP       - Kernel density estimator for one dimensional distribution.
%    HAMMERSLEY    - Hammersley quasi-random sequence
%


function fx = p06_fun ( n, x )

%*****************************************************************************80
%
%% P06_FUN evaluates the integrand for problem 6.
%
%  Interval:
%
%    -1 <= x <= 1
%
%  Integrand:
%
%    sqrt ( abs ( x + 0.5 ) )
%
%  Exact Integral:
%
%    ( sqrt ( 2 ) + 3 * sqrt ( 6 ) ) / 6 = 1.460447131787105
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Charles Clenshaw, Alan Curtis,
%    A Method for Numerical Integration on an Automatic Computer,
%    Numerische Mathematik,
%    Volume 2, Number 1, December 1960, pages 197-205.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  fx = sqrt ( abs ( x + 0.5 ) );

  return
end


function alpha = sphericalAngle(p1, p2, p3)
%SPHERICALANGLE Compute angle between points on the sphere
%
%   ALPHA = sphericalAngle(P1, P2, P3)
%   Computes angle (P1, P2, P3), i.e. the angle, measured at point P2,
%   between the direction (P2, P1) and the direction (P2, P3).
%   The result is given in radians, between 0 and 2*PI.
%
%   Points are given either as [x y z] (there will be normalized to lie on
%   the unit sphere), or as [phi theta], with phi being the longitude in [0
%   2*PI] and theta being the elevation on horizontal [-pi/2 pi/2].
%
%
%   NOTE:
%   this is an 'oriented' version of the angle computation, that is, the
%   result of sphericalAngle(P1, P2, P3) equals
%   2*pi-sphericalAngle(P3,P2,P1). To have the more classical relation
%   (with results given betwen 0 and PI), it suffices to take the minimum
%   of angle and 2*pi-angle.
%
%   Examples
%     % Use inputs as cartesian coordinates
%     p1 = [0 1 0];
%     p2 = [1 0 0];
%     p3 = [0 0 1];
%     alpha = sphericalAngle(p1, p2, p3)
%     alpha =
%         1.5708
%
%     % Use inputs as spherical coordinates
%     sph1 = [.1 0];
%     sph2 = [0 0];
%     sph3 = [0 .1];
%     alphas = sphericalAngle(sph1, sph2, sph3)
%     alphas =
%         1.5708
%
%
%   See also:
%   geom3d, angles3d, spheres, sph2cart

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY
%   23-05-2006 fix bug for points with angle from center > pi/2
%   05-06-2013 fix bug for points given as spherical coordinates, better
%       support for multiple inputs

% test if points are given as matlab spherical coordinates
if size(p1, 2) == 2
    [x, y, z] = sph2cart(p1(:,1), p1(:,2), ones(size(p1,1), 1));
    p1 = [x y z];
    [x, y, z] = sph2cart(p2(:,1), p2(:,2), ones(size(p2,1), 1));
    p2 = [x y z];
    [x, y, z] = sph2cart(p3(:,1), p3(:,2), ones(size(p3,1), 1));
    p3 = [x y z];
end

% normalize points
p1  = normalizeVector3d(p1);
p2  = normalizeVector3d(p2);
p3  = normalizeVector3d(p3);

% create the plane tangent to the unit sphere and containing central point
plane = createPlane(p2, p2);

% project the two other points on the plane
pp1 = planePosition(projPointOnPlane(p1, plane), plane);
pp3 = planePosition(projPointOnPlane(p3, plane), plane);

% compute angle on the tangent plane
pp2 = zeros(max(size(pp1, 1), size(pp3,1)), 2);
alpha = angle3Points(pp1, pp2, pp3);


function x = f2_abscissas ( n )

%*****************************************************************************80
%
%% F2_ABSCISSAS computes Fejer Type 2 abscissas.
%
%  Discussion:
%
%    The interval is [-1,+1].
%
%    The abscissas are the cosines of equally spaced angles.
%    The angles are computed as N+2 equally spaced values between 0 and PI,
%    but with the first and last angle omitted.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%    Walter Gautschi,
%    Numerical Quadrature in the Presence of a Singularity,
%    SIAM Journal on Numerical Analysis,
%    Volume 4, Number 3, 1967, pages 357-362.
%
%    Joerg Waldvogel,
%    Fast Construction of the Fejer and Clenshaw-Curtis Quadrature Rules,
%    BIT Numerical Mathematics,
%    Volume 43, Number 1, 2003, pages 1-18.
%
%  Parameters:
%
%    Input, integer N, the order of the rule.
%
%    Output, real X(N), the abscissas.
%
  if ( n == 1 )
    x(1) = 0.0;
    return
  elseif ( n == 2 )
    x(1) = -0.5;
    x(2) =  0.5;
    return
  end

  for i = 1 : n
    theta(i) = ( n + 1 - i ) * pi ...
             / ( n + 1     );
  end

  x(1:n) = cos ( theta(1:n) );

  return
end


function mat = eulerAnglesToRotation3d(phi, theta, psi, varargin)
%EULERANGLESTOROTATION3D Convert 3D Euler angles to 3D rotation matrix.
%
%   MAT = eulerAnglesToRotation3d(PHI, THETA, PSI)
%   Creates a rotation matrix from the 3 euler angles PHI THETA and PSI,
%   given in degrees, using the 'XYZ' convention (local basis), or the
%   'ZYX' convention (global basis). The result MAT is a 4-by-4 rotation
%   matrix in homogeneous coordinates.
%
%   PHI:    rotation angle around Z-axis, in degrees, corresponding to the
%       'Yaw'. PHI is between -180 and +180.
%   THETA:  rotation angle around Y-axis, in degrees, corresponding to the
%       'Pitch'. THETA is between -90 and +90.
%   PSI:    rotation angle around X-axis, in degrees, corresponding to the
%       'Roll'. PSI is between -180 and +180.
%   These angles correspond to the "Yaw-Pitch-Roll" convention, also known
%   as "Tait–Bryan angles".
%
%   The resulting rotation is equivalent to a rotation around X-axis by an
%   angle PSI, followed by a rotation around the Y-axis by an angle THETA,
%   followed by a rotation around the Z-axis by an angle PHI.
%   That is:
%       ROT = Rz * Ry * Rx;
%
%   MAT = eulerAnglesToRotation3d(ANGLES)
%   Concatenates all angles in a single 1-by-3 array.
%
%   ... = eulerAnglesToRotation3d(ANGLES, CONVENTION)
%   CONVENTION specifies the axis rotation sequence.
%   Supported conventions are: 'ZYX', 'ZYZ'. Default is 'ZYX'.
%
%   Example
%   [n e f] = createCube;
%   phi     = 20;
%   theta   = 30;
%   psi     = 10;
%   rot = eulerAnglesToRotation3d(phi, theta, psi);
%   n2 = transformPoint3d(n, rot);
%   drawPolyhedron(n2, f);
%
%   See also
%   transforms3d, createRotationOx, createRotationOy, createRotationOz
%   rotation3dAxisAndAngle
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-07-22,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%   HISTORY
%   2011-06-20 rename and use degrees

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

% Process input arguments
if size(phi, 2) == 3
    % manages arguments given as one array
    psi     = phi(:, 3);
    theta   = phi(:, 2);
    phi     = phi(:, 1);
end

% create individual rotation matrices
k = pi / 180;

switch convention
    case 'ZYX'
        rot1 = createRotationOx(psi * k);
        rot2 = createRotationOy(theta * k);
        rot3 = createRotationOz(phi * k);
    case 'ZYZ'
        rot1 = createRotationOz(psi * k);
        rot2 = createRotationOy(theta * k);
        rot3 = createRotationOz(phi * k);
end

% concatenate matrices
mat = rot3 * rot2 * rot1;


function varargout = circumCenter(a, b, c)
%CIRCUMCENTER  Circumcenter of three points
%
%   CC = circumCenter(P1, P2, P3)
%
%   Example
%     A = [10 10]; B = [30 10]; C = [10 20];
%     circumCenter(A, B, C)
%     ans =
%         20    15
%
%     % works also for multiple input points
%     circumCenter([A;A;A], [B;B;B], [C;C;C])
%     ans =
%         20    15
%         20    15
%         20    15
%
%
%   See also
%     points2d, circles2d, circumCircle, centroid
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2011-12-09,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% pre-compute some terms
ah = sum(a .^ 2, 2);
bh = sum(b .^ 2, 2);
ch = sum(c .^ 2, 2);

dab = a - b;
dbc = b - c;
dca = c - a;

% common denominator
D  = .5 ./ (a(:,1) .* dbc(:,2) + b(:,1) .* dca(:,2) + c(:,1) .* dab(:,2));

% center coordinates
xc =  (ah .* dbc(:,2) + bh .* dca(:,2) + ch .* dab(:,2) ) .* D;
yc = -(ah .* dbc(:,1) + bh .* dca(:,1) + ch .* dab(:,1) ) .* D;

if nargout <= 1
    varargout = {[xc yc]};
else
    varargout = {xc, yc};
end


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

M=[                                     0,                                          0,   0.35446557777227471722730849524904581806,   0.30331168845504517111391728481208001144;
                                        0,                                          0,  -0.74972609378250711033655604388057044149,   0.55168907357213937275730716433358729608;
  0.6505815563982325146829577797417295398,                                          0,  -0.35523170084357268589593075201816127231,   0.28353223437153468006819777082540613962;
                                        0,    0.6505815563982325146829577797417295398,  -0.35523170084357268589593075201816127231,   0.28353223437153468006819777082540613962;
 -0.6505815563982325146829577797417295398,                                          0,  -0.35523170084357268589593075201816127231,   0.28353223437153468006819777082540613962;
                                        0,   -0.6505815563982325146829577797417295398,  -0.35523170084357268589593075201816127231,   0.28353223437153468006819777082540613962;
 0.65796699712169008954533549931479427127,   0.65796699712169008954533549931479427127,  -0.92150343220236930457646242598412224897,   0.16938424178833585063066278355484370017;
 0.65796699712169008954533549931479427127,  -0.65796699712169008954533549931479427127,  -0.92150343220236930457646242598412224897,   0.16938424178833585063066278355484370017;
-0.65796699712169008954533549931479427127,   0.65796699712169008954533549931479427127,  -0.92150343220236930457646242598412224897,   0.16938424178833585063066278355484370017;
-0.65796699712169008954533549931479427127,  -0.65796699712169008954533549931479427127,  -0.92150343220236930457646242598412224897,   0.16938424178833585063066278355484370017];

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 [Fn,Ln] = fibonacci(n,modulus)
% fibonacci: vpi tool to efficiently compute the n'th Fibonacci number and the n'th Lucas number
% usage: [Fn,Ln] = fibonacci(n)
% usage: [Fn,Ln] = fibonacci(n,modulus)
%
% Compute the nth Fibonacci number as well as the nth Lucas
% Lucas number. In the event that all members of these
% sequences from 1 to n are desired, then a simple,
% direct loop would be more efficient, and more direct.
%
% Both the Fibonacci numbers and the Lucas numbers
% are defined by the same basic recursion:
%
%  F(n) = F(n-1) + F(n-2)
%  L(n) = L(n-1) + L(n-2)
%
% The difference is the starting point. The Fibonacci
% numbers start with F(1) = F(2) = 1, whereas the Lucas
% sequence starts with L(1) = 1, and L(2) = 3. The first
% few members of these sequences are:
%
%  Fibonacci: [1 1 2 3 5 8 13 21 ... ]
%  Lucas:     [1 3 4 7 11 18 29 ... ]
%
% These sequences are also defined for n = 0 and for
% negative values of n.
%
% For efficiency, fibonacci uses a variety of tricks to
% maximize speed. While computation of fibonacci numbers
% is commonly done recursively, fibonacci does so using a
% direct iterative scheme given the binary representation
% of n. In addition, several Fibonacci and Lucas number
% identities are employed to maximize throughput.
%
% The methods employed by fibonacci will be O(log2(n)) in time.
%
%
% Arguments: (input)
%  n - any non-negative integer, vpi or numeric.
%      n must be less than 2^53 (in theory. Even that
%      number would be impossibly huge to compute.
%      Don't bother to try it.) In practice, recognize
%      that F(1e6) is a number with 208995 digits.
%
%      When n is a vector or array, fibonacci will
%      generate each of the indicated Fibonacci and
%      Lucas numbers.
%
%  modulus - (OPTIONAL) - allows the computation of the
%      indicated Fibonacci/Lucas numbers modulo a given
%      modulus. This enables the computation of such
%      numbers for truly immense index.
%
% Arguments: (output)
%  Fn, Ln - scalar vpi number, containing the nth
%      Fibonacci number and nth Lucas numbers in
%      their respective sequences.
%
% Example:
%  [Fn,Ln] = fibonacci(150)
%
% Fn =
%    9969216677189303386214405760200
% Ln =
%    22291846172619859445381409012498
%
%  See also:
%
%  Author: John D'Errico
%  e-mail: woodchips@rochester.rr.com
%  Release: 1.0
%  Release date: 4/30/09

if (nargin < 1) || (nargin > 2)
  error('fibonacci accepts only 1 or 2 arguments')
elseif any(n(:)~=round(n(:))) || any(abs(n) > 2^53)
  error('n must be an integer, <= 2^53 in absolute value')
end

% was a modulus provided?
if (nargin < 2)
  modulus = [];
end

% The first 15 Fibonacci and Lucas numbers to
% start things off efficiently.
Fseq = [0 1 1 2 3  5  8 13 21 34  55  89 144 233 377  610];
Lseq = [2 1 3 4 7 11 18 29 47 76 123 199 322 521 843 1364];

if ~isempty(modulus)
  Fseq = mod(Fseq,modulus);
  Lseq = mod(Lseq,modulus);
end

% intialize Fn and Ln to the proper size,
% in case n is a vector or array
Fn = repmat(vpi(0),size(n));
Ln = Fn;

% much faster if n is not a vpi but a double.
% also, we don't need to worry about n being
% larger than 2^53, as this would have a vast
% number of digits.
n = double(n(:));

% catch any zeros in n first.
k = (n(:) == 0);
% Fn(k) is already zero
if any(k)
  Ln(k) = vpi(2);
end

% Negative values for n will be inconvenient
% in a loop, so make them all positive. deal
% with any signs later.
nsign = 2*(n>=0) - 1;
if any(n<0)
  neven = iseven(n);
  n = abs(n);
end

if numel(n) > 1
  % sort them and work upwards
  [n,ntags] = sort(n);

  flag = false;
  for i = 1:numel(n)
    if n(i) <= 15
      % small values of n are pre-computed
      Fn(i) = Fseq(n(i) + 1);
      Ln(i) = Lseq(n(i) + 1);
    elseif (i == 1) || ((n(i) - n(i-1)) > 15)
      % the smallest value of n is > 15,
      % or the difference from the last value
      % computed was too large
      [Fn(i),Ln(i)] = fibonacci(n(i),modulus);

      flag = false;
    elseif n(i) == n(i-1)
      % will happen if there were positive and
      % negative values in the list
      Fn(i) = Fn(i-1);
      Ln(i) = Ln(i-1);

      flag = false;
    elseif (n(i) == (n(i-1)+1))
      % we can use an addition formula to
      % get Fn & Ln. Which one?
      if flag
        % the last two values of n were
        % separated by 1, so just use the
        % two term recurrence
        Fn(i) = (Fn(i-1) + Fn(i-2));
        Ln(i) = (Ln(i-1) + Ln(i-2));
        if ~isempty(modulus)
          Fn(i) = mod(Fn(i),modulus);
          Ln(i) = mod(Ln(i),modulus);
        end
      else
        % we must use the addition formula
        Fn(i) = (Fn(i-1) + Ln(i-1))./2;
        Ln(i) = (5 .*Fn(i-1) + Ln(i-1))./2;
        if ~isempty(modulus)
          Fn(i) = mod(Fn(i),modulus);
          Ln(i) = mod(Ln(i),modulus);
        end

        % flag indicates whether the last two
        % numbers were consecutive
        flag = true;
      end

    else
      % 1 < n(i) - n(i-1) <= 15
      m = n(i) - n(i-1);
      Fm = Fseq(m+1);
      Lm = Lseq(m+1);

      % use an addition formula
      Fn(i) = (Fm.*Ln(i-1) + Lm.*Fn(i-1))./2;
      Ln(i) = (5 .*Fm.*Fn(i-1) + Lm.*Ln(i-1))./2;
      if ~isempty(modulus)
        Fn(i) = mod(Fn(i),modulus);
        Ln(i) = mod(Ln(i),modulus);
      end

      flag = false;
    end

  end % for i = 1:numel(n)

  % shuffle Fn and Ln to reflect the sort
  Fn(ntags) = Fn;
  Ln(ntags) = Ln;

else
  % For only one value of n, compute it individually.
  % Uses an efficient iterative (recursive, but not
  % really so) scheme.

  % get the binary representation of n. Thus
  % nbin is a character vector, of length
  % ceil(log2(n)).
  nbin = dec2bin(n);

  % get the 4 highest order bits from nbin
  k = min(numel(nbin),4);

  % start the sequence from the top
  % few bits of n.
  nhigh = bin2dec(nbin(1:k));

  Fn = vpi(Fseq(nhigh+1));
  Ln = vpi(Lseq(nhigh+1));

  % We need to loop forwards. Essentially, we started
  % with the highest order bit(s) of the binary representation
  % for n. Look at each successively lower order bit.
  % If the next bit is 0, then we are essentially doubling
  % the index at this step. If the next bit is odd, then
  % we are moving to 2*n+1.
  for k = 5:numel(nbin)
    bit = (nbin(k) == '1');
    if bit
      % the next bit was odd. Use
      % a 2*n+1 rule to step up.
      F2n = Fn.*Ln;
      % we want to do this...
      %   L2n = (5 .*Fn.*Fn + Ln.*Ln)./2;
      % Instead use the identity that
      %   5F(n)^2 + L(n)^2 = 2*L(n)^2 + 4*(-1)^(n+1)
      % to make that expression more efficiently
      % computed. See that the form used below
      % has only a single multiplication between a
      % pair of large integers, whereas the prior
      % form for L2n would have had several multiples
      % as well as a divide.
      L2n = Ln.*Ln + 2*(-1)^(nhigh+1);

      Fn = (L2n + F2n)./2;
      Ln = (5 .*F2n + L2n)./2;
      if ~isempty(modulus)
        Fn = mod(Fn,modulus);
        Ln = mod(Ln,modulus);
      end
    else
      % the next bit was even. Use the 2*n
      % rule to step up.
      F2n = Fn.*Ln;
      Ln = Ln.*Ln + 2*(-1)^(nhigh+1);
      Fn = F2n;
      if ~isempty(modulus)
        Fn = mod(Fn,modulus);
        Ln = mod(Ln,modulus);
      end
    end

    % update the top bits of n
    nhigh = 2*nhigh + bit;
  end % for k = 5:numel(nbin)

end % if numel(n) > 1

% if n was negative, then we may need to apply a
% sign change to Fn and Ln.
if any(nsign < 0)
  k = neven & (nsign < 0);
  Fn(k) = -Fn(k);

  k = (~neven) & (nsign < 0);
  Ln(k) = -Ln(k);

  if ~isempty(modulus)
    Fn(k) = mod(Fn(k),modulus);
    Ln(k) = mod(Ln(k),modulus);
  end
end

% ==================================
% End mainline, begin subfunctions.
% ==================================

function result = iseven(n)
% tests if a scalar value is an even integer, works
% for either numeric or vpi inputs
if isnumeric(n)
  result = (mod(n,2) == 0);
elseif isa(n,'vpi')
  % must have been a vpi
  result = (mod(trailingdigit(n,1),2) == 0);
else
  error('n must be either numeric or vpi')
end


function [midpoint,minDist,t]=closestPointBetween2Lines(a1,b1,a2,b2)
%%CLOSESTPOINTBETWEEN2LINES Given two non-parallel lines in numDim
%              dimensions that do not necessarily intersect, find the point
%              closest to both lines. This is done by finding the point on
%              each line that is closest to the other line, and then
%              averaging the points (get the point halfway between the
%              lines).
%
%INPUTS: a1,b1 numDimXN matrix of parameters for the first line in N pairs
%              of lines, the closes point between each being desired. The
%              ith line is represented in parametic form as
%              r=a1(:,i)*t+b1(:,i), where t is a scalar parameter and p is
%              a point on the line.
%        a2,b2 The numDimXN set of parameters for the second lines.
%
%OUTPUTS: midpoint The numDimXN set of closest points between each of the N
%                  pairs of lines.
%          minDist The NX1 set of minimum distances between the pairs of
%                  lines.
%                t The 2XN set of values of t used in the linear equations
%                  to get the minimum points. These values can be positive
%                  or negative.
%
%Line 1 is r1=a1*t1+b1 and line 2 is r2=a2*t2+b2. We want to find the
%values of t1 and t2 such that norm(r1-r2)^2 is minimized.
%norm(r1-r2)^2=norm(a1*t1+b1-a2*t2-b2)^2
%             =norm(A*T+c)^2 where A=[a1,-a2] and c=b1-b2 and T=[t1;t2]
%             =T'*A'*A*T+2*c'*A*T+c'*c
%Setting the derivative with respect to the vector T equal to zero, we get
%             2*A'*A*T+2*A'*c=0
%The solution is T=-inv(A'*A)*A'*c, which (being the zero deivative point)
%is an extrema and being the only extrema (unless the lines are parallel)
%is the closest point between the lines. The pinv function is used to
%evaluate (A'*A)\A' as it is more stable.
%
%EXAMPLE 1:
%This is a simple 2D case, so the lines intersect. The starting points are
%drawn in blue and lines with positive t are drawn in blue. The
%intersection point is marked in red.
% x1=[4;3];
% u1=[3;2];
% u1=u1/norm(u1);
% x2=[6;7];
% u2=[6;-1];
% u2=u2/norm(u2);
% r=10;
% p1=[x1,x1+r*u1];
% p2=[x2,x2+r*u2];
% figure(1)
% clf
% hold on
% scatter(x1(1),x1(2),400,'.b')
% scatter(x2(1),x2(2),400,'.b')
% plot(p1(1,:),p1(2,:),'-b')
% plot(p2(1,:),p2(2,:),'-b')
% pt=closestPointBetween2Lines(u1,x1,u2,x2);
% scatter(pt(1),pt(2),300,'.r')
%
%EXAMPLE 2:
%This is the same as example 1, but is in 3D. This time, the lines do not
%intersect in 3D, so we use t to compute the two nearest points on either
%line and we draw a red line between them.
% x1=[4;3;8];
% u1=[3;2;0];
% u1=u1/norm(u1);
% x2=[6;7;-2];
% u2=[6;-1;4];
% u2=u2/norm(u2);
%
% r=20;
% p1=[x1,x1+r*u1];
% p2=[x2,x2+r*u2];
% figure(1)
% clf
% hold on
% scatter3(x1(1),x1(2),x1(3),400,'.b')
% scatter3(x2(1),x2(2),x2(3),400,'.b')
% plot3(p1(1,:),p1(2,:),p1(3,:),'-b')
% plot3(p2(1,:),p2(2,:),p2(3,:),'-b')
% [p,~,t]=closestPointBetween2Lines(u1,x1,u2,x2);
% scatter3(p(1),p(2),p(3),200,'.r')
% pt1=x1+t(1)*u1;
% pt2=x2+t(2)*u2;
% plot3([pt1(1);pt2(1)],[pt1(2);pt2(2)],[pt1(3);pt2(3)],'-r')
% view(8,90)
%
%March 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(a1,2);
numDim=size(a1,1);

minDist=zeros(N,1);
midpoint=zeros(numDim,N);
t=zeros(2,N);
for i=1:N
    A=[a1(:,i),-a2(:,i)];
    c=b1(:,i)-b2(:,i);

    %pinv(A)=(A'*A)\A', we are doing pinv(A)*c using lsqminnorm.
    TMin=-lsqminnorm(A,c);

    t(:,i)=TMin;

    r1Min=a1*TMin(1)+b1;
    r2Min=a2*TMin(2)+b2;

    minDist(i)=norm(r1Min-r2Min);
    midpoint(:,i)=(r1Min+r2Min)/2;
end
end

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


function p = predict(Theta1, Theta2, X)
%PREDICT Predict the label of an input given a trained neural network
%   p = PREDICT(Theta1, Theta2, X) outputs the predicted label of X given the
%   trained weights of a neural network (Theta1, Theta2)

% Useful values
m = size(X, 1);
num_labels = size(Theta2, 1);

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

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned neural network. You should set p to a
%               vector containing labels between 1 to num_labels.
%
% Hint: The max function might come in useful. In particular, the max
%       function can also return the index of the max element, for more
%       information see 'help max'. If your examples are in rows, then, you
%       can use max(A, [], 2) to obtain the max for each row.
%

a1 = [ones(m, 1) X]; % add 1

z2 = a1 * Theta1';

a2 = [ones(size(sigmoid(z2), 1), 1) sigmoid(z2)]; % add 1

z3 = a2 * Theta2';
a3 = sigmoid(z3); % H_theta(x)

[x, ix] = max(a3, [], 2);

p = ix;

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

end


function [ n_data, n, c ] = omega_values ( n_data )

%*****************************************************************************80
%
%% OMEGA_VALUES returns some values of the OMEGA function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by
%
%      Length [ FactorInteger [ n ] ]
%
%  First values:
%
%     N   OMEGA(N)
%
%     1    0
%     2    1
%     3    1
%     4    1
%     5    1
%     6    2
%     7    1
%     8    1
%     9    1
%    10    2
%    11    1
%    12    2
%    13    1
%    14    2
%    15    2
%    16    1
%    17    1
%    18    2
%    19    1
%    20    2
%
%  Formula:
%
%    If N = 1, then
%
%      OMEGA(N) = 0
%
%    else if the prime factorization of N is
%
%      N = P1^E1 * P2^E2 * ... * PM^EM,
%
%    then
%
%      OMEGA(N) = M
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the argument of the OMEGA function.
%
%    Output, integer C, the value of the OMEGA function.
%
  n_max = 23;

  c_vec = [ ...
      0,   1,   1,   1,   1, ...
      2,   1,   1,   1,   2, ...
      3,   1,   4,   4,   3, ...
      1,   5,   2,   2,   1, ...
      6,   7,   8 ];

  n_vec = [ ...
           1, ...
           2, ...
           3, ...
           4, ...
           5, ...
           6, ...
           7, ...
           8, ...
           9, ...
          10, ...
          30, ...
         101, ...
         210, ...
        1320, ...
        1764, ...
        2003, ...
        2310, ...
        2827, ...
        8717, ...
       12553, ...
       30030, ...
      510510, ...
     9699690 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    c = 0;
  else
    n = n_vec(n_data);
    c = c_vec(n_data);
  end

  return
end


function [R,t,PfromTransformed]=rigidTransformation(Pfrom,Pto,varargin)
%% Find the optimal rigid body transformation between two point clouds %%%%%%%%%
% The optimal solution is the least square minimization solution between
% the two point clouds. See (Rubin & Solav 2016)
% The two point clouds must be corresponded (indeces refer to the same point)

%% INPUT:
% Pfrom=  point cloud (to be transformed into TruP)
% Pto= true 3D point cloud
% * optional input:
% estimation method: 'rigid' (default) or 'affine'

%% OUTPUT:
% R = estimated rotation matrix from Pfrom to Pto (3-by-3)
% t = estimated translation vector (3-by-1)
% PfromTransformed = Pfrom transformed using R and t

%%
nArg=numel(varargin);

switch nArg
    case 0
        estimationMethod='rigid';
    case 1
        estimationMethod=varargin{1};
end

%%
a=Pfrom;
b=Pto;

% find NaNs
aNanInd=find(isnan(a(:,1)));
bNanInd=find(isnan(b(:,1)));
abNanInd=[aNanInd; bNanInd];
% delete Nans
aNoNan=a;
aNoNan(abNanInd,:)=[];
bNoNan=b;
bNoNan(abNanInd,:)=[];

ac=(1/length(aNoNan))*sum(aNoNan); % centroid of  RecP.
bc=(1/length(bNoNan))*sum(bNoNan); % centroid of  TruP.

da=aNoNan-ac;
db=bNoNan-bc;

switch estimationMethod

    case 'rigid'
        M=db'*da;
        [U,~,V] = svd(M);
        S=[1 0 0; 0 1 0; 0 0 det(U*V')];
        R=U*S*V'; %rotation matrix from reconstructed coordinates to true
        t=bc'-R*ac'; %translation vector from centroid of reconstructed coordinates to true- this is the translation of the origin of the coordinate system!

    case 'affine'
        M=db'*da;
        A=da'*da;
        F=M/A;
%         R=F*(F'*F)^-.5; % this gives the same solution as the svd
        [U,~,V] = svd(F);
        S=[1 0 0; 0 1 0; 0 0 det(U*V')];
        R=U*S*V'; %rotation matrix from reconstructed coordinates to true
        t=bc'-R*ac'; %translation vector from centroid of reconstructed coordinates to true- this is the translation of the origin of the coordinate system!

end

PfromTransformed=(R*a'+t)'; %transformed constructed coordinates

% residuals=PfromTransformed-Pto;
% residualsMgn=sqrt(sum(residuals.^2,2));
% residulasMgnRMS=sqrt(nansum(residualsMgn.^2)/sum(~isnan(residualsMgn)));

end

%%
% MultiDIC: a MATLAB Toolbox for Multi-View 3D Digital Image Correlation
%
% License: <https://github.com/MultiDIC/MultiDIC/blob/master/LICENSE.txt>
%
% Copyright (C) 2018  Dana Solav
%
% If you use the toolbox/function for your research, please cite our paper:
% <https://engrxiv.org/fv47e>


function ta = acf(y,p)
% ACF - Compute Autocorrelations Through p Lags
% >> myacf = acf(y,p)
%
% Inputs:
% y - series to compute acf for, nx1 column vector
% p - total number of lags, 1x1 integer
%
% Output:
% myacf - px1 vector containing autocorrelations
%        (First lag computed is lag 1. Lag 0 not computed)
%
%
% A bar graph of the autocorrelations is also produced, with
% rejection region bands for testing individual autocorrelations = 0.
%
% Note that lag 0 autocorelation is not computed,
% and is not shown on this graph.
%
% Example:
% >> acf(randn(100,1), 10)
%

% --------------------------
% USER INPUT CHECKS
% --------------------------

[n1, n2] = size(y) ;
if n2 ~=1
    error('Input series y must be an nx1 column vector')
end

[a1, a2] = size(p) ;
if ~((a1==1 & a2==1) & (p<n1))
    error('Input number of lags p must be a 1x1 scalar, and must be less than length of series y')
end

% -------------
% BEGIN CODE
% -------------

ta = zeros(p,1) ;
global N
N = max(size(y)) ;
global ybar
ybar = mean(y);

% Collect ACFs at each lag i
for i = 1:p
   ta(i) = acf_k(y,i) ;
end

% Plot ACF
% Plot rejection region lines for test of individual autocorrelations
% H_0: rho(tau) = 0 at alpha=.05
bar(ta)
line([0 p+.5], (1.96)*(1/sqrt(N))*ones(1,2))
line([0 p+.5], (-1.96)*(1/sqrt(N))*ones(1,2))

% Some figure properties
line_hi = (1.96)*(1/sqrt(N))+.05;
line_lo = -(1.96)*(1/sqrt(N))-.05;
bar_hi = max(ta)+.05 ;
bar_lo = -max(ta)-.05 ;

if (abs(line_hi) > abs(bar_hi)) % if rejection lines might not appear on graph
    axis([0 p+.60 line_lo line_hi])
else
    axis([0 p+.60 bar_lo bar_hi])
end
title({' ','Sample Autocorrelations',' '})
xlabel('Lag Length')
set(gca,'YTick',[-1:.20:1])
% set number of lag labels shown
if (p<28 & p>4)
    set(gca,'XTick',floor(linspace(1,p,4)))
elseif (p>=28)
    set(gca,'XTick',floor(linspace(1,p,8)))
end
set(gca,'TickLength',[0 0])

% ---------------
% SUB FUNCTION
% ---------------
function ta2 = acf_k(y,k)
% ACF_K - Autocorrelation at Lag k
% acf(y,k)
%
% Inputs:
% y - series to compute acf for
% k - which lag to compute acf
%
global ybar
global N
cross_sum = zeros(N-k,1) ;

% Numerator, unscaled covariance
for i = (k+1):N
    cross_sum(i) = (y(i)-ybar)*(y(i-k)-ybar) ;
end

% Denominator, unscaled variance
yvar = (y-ybar)'*(y-ybar) ;

ta2 = sum(cross_sum) / yvar ;


function an=random_unit_vector(varargin)
% random_unit_vector
% random_unit_vector(n)
% random_unit_vector(m,n)
% random_unit_vector([m n])
% random_unit_vector('double')
% random_unit_vector(n,'double')
% random_unit_vector(m,n,'double')
% random_unit_vector([m n],'double')
% random_unit_vector('single')
% random_unit_vector(n,'single')
% random_unit_vector(m,n,'single')
% random_unit_vector([m n],'single')

% m - dimentionarity
% n - number of unit vectors

md=3; % default m

isg=false; % if single
switch nargin
    case 0
        % if no inputs
        n=1;
        m=md;
    case 1
        %
        i1=varargin{1};
        if ischar(i1)
            if strcmpi(i1,'single')
                isg=true;
            end
        else
            if length(i1)==1
                m=md;
                n=i1;
            else
                m=i1(1);
                n=i1(2);
            end
        end
    case 2
        i1=varargin{1};
        i2=varargin{2};
        if ischar(i2)
            if length(i1)==1
                m=md;
                n=i1;
            else
                m=i1(1);
                n=i1(2);
            end
            if strcmpi(i2,'single')
                isg=true;
            end
        else
            m=i1;
            n=i2;
        end
    case 3
        m=varargin{1};
        n=varargin{2};
        if strcmpi(varargin{3},'single')
            isg=true;
        end

end

% simple case of 1d
if m==1
    if isg
        an=single(2*(randn(1,n)>0)-1);
    else
        an=2*(randn(1,n)>0)-1;
    end
    return;
end

if isg
    v=randn(m,n,'single');
else
    v=randn(m,n);
end

% normalize:
if isg
    an=zeros(m,n,'single');
else
    an=zeros(m,n);
end
for nc=1:n
    while 1
        v2=v(:,nc)'*v(:,nc);
        if v2>1e-10 % too small values must be excluded
            % because it will have discretization errors
            an(:,nc)=v(:,nc)/sqrt(v2);
            break;
        else
            % otherwise repeat random generation
            if isg
                v(:,nc)=randn(m,1,'single');
            else
                v(:,nc)=randn(m,1);
            end
        end
    end
end


%KDGAUSS Derivative of Gaussian kernel
%
% K = KDGAUSS(SIGMA) is a 2-dimensional derivative of Gaussian kernel (WxW)
% of width (standard deviation) SIGMA and centred within the matrix K whose
% half-width H = 3xSIGMA and W=2xH+1.
%
% K = KDGAUSS(SIGMA, H) as above but the half-width is explictly specified.
%
% Notes::
% - This kernel is the horizontal derivative of the Gaussian, dG/dx.
% - The vertical derivative, dG/dy, is K'.
% - This kernel is an effective edge detector.
%
% See also KGAUSS, KDOG, KLOG, ISOBEL, ICONV.

% 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 = kdgauss(sigma, w)

    if nargin == 1,
        w = ceil(3*sigma);
    end
    ww = 2*w + 1;

    [x,y] = meshgrid(-w:w, -w:w);

    % This should properly be
    %   m = -x/sigma^4 /(2*pi) .*  exp( -(x.^2 + y.^2)/2/sigma^2);
    % but the effect of the error is simply to scale the result by sigma^2.
    %
    % Too many results in the book depend on this...

    m = -x/sigma^2 /(2*pi) .*  exp( -(x.^2 + y.^2)/2/sigma^2);


% MAIN.m -- Robot Arm Trajectory
%
% Given an initial and final state, as well as joint angle and rate
% constraints, find the minimum-jerk trajectory that solves the boundary
% value problem.
%
%

n = 15;  %Order of interpolating polynomial;

% Problem specifications:
tStart = 0;
tFinal = 1;
qLow = -pi;
qUpp = pi;
dqMax = 1.5;   %Joint speed limit

% Boundary values
qStart = 0;
qFinal = 1;
dqStart = 0;
dqFinal = -1.5;

% Derivatives
D = chebyshevDifferentiationMatrix(n,[tStart,tFinal]);   %Rates
DD = D*D;    %Accelerations
DDD = DD*D;   %Jerk

% Chebyshev grid and quadrature weights:
[t,w] = chebyshevPoints(n,[tStart,tFinal]);

%%%% Quadratic Program:
%   min 0.5*x'*H*x + f'*x   subject to:  A*x <= b
%    x                                   Aeq*x == beq
%                                        LB <= x <=UB

%%%% Boundary Values:
Aeq = zeros(4,n); beq = zeros(4,1);
Aeq(1,1) = 1; beq(1) = qStart;
Aeq(2,end) = 1; beq(2) = qFinal;
Aeq(3:4,:) = D([1,end],:); beq(3:4) = [dqStart; dqFinal];

%%%% Dynamics:
% u == DDD*x;   %Equality Constraint - expressed directly in cost function

%%%% Joint Rate Limits:
% -dqMax <= D*x <= dqMax
A = [D;-D];
b = [dqMax*ones(n,1); dqMax*ones(n,1)];

%%%% Joint Angle Limits:
% qLow <= x <= qUpp
LB = qLow*ones(n,1);
UB = qUpp*ones(n,1);

%%%% Objective Function:
% min  w*(u^2) = x'*(DDD'*W*DDD);  W = diag(w);
W = diag(w);
H = 0.5*(DDD'*W*DDD);

%%%% Initialize:    (Linear trajectory)
xGuess = interp1([tStart;tFinal],[qStart;qFinal],t');

%%%% Options:
options = optimset(...
    'Display','iter',...  % {'iter','final'}
    'Algorithm','interior-point-convex');

%%%% Build Problem:
problem.H = H;
problem.f = zeros(n,1);
problem.Aineq = A;
problem.bineq = b;
problem.Aeq = Aeq;
problem.beq = beq;
problem.lb = LB;
problem.ub = UB;
problem.x0 = xGuess;
problem.options = options;
problem.solver = 'quadprog';

%%%% Solve!
[x, fVal, exitFlag, output] = quadprog(problem);

% Plot the solution:
tt = linspace(t(1),t(end),100);
[xx, dxx] = chebyshevInterpolate(x',tt,[t(1),t(end)]);
figure(2); clf;
subplot(2,1,1); hold on;
plot(tt,xx);
plot(t,x,'ko');
subplot(2,1,2); hold on;
plot(tt,dxx);
plot(t,D*x,'ko');


function sphere = createSphere(varargin)
%CREATESPHERE Create a sphere containing 4 points.
%
%   s = createSphere(p1, p2, p3, p4);
%   return in s the sphere common to the 4 pointsp1, p2, p3 and p4.
%
%   Ref: P. Bourke
%   http://astronomy.swin.edu.au/~pbourke/geometry/spherefrom4/
%
%   See also
%   spheres, circles3d

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-03-22
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

if length(varargin)==4
    pts = [varargin{1};varargin{2};varargin{3};varargin{4}];
elseif length(varargin)==1
    pts = varargin{1};
else
    error('wrong number of arguments in createSphere');
end

m1 = det([pts ones(4,1)]);
s2 = sum(pts.*pts, 2);
m2 = det([s2 pts(:,2) pts(:,3) ones(4,1)]);
m3 = det([pts(:,1) s2 pts(:,3) ones(4,1)]);
m4 = det([pts(:,1) pts(:,2) s2 ones(4,1)]);

m5 = det([s2 pts]);

x0 = m2*.5/m1;
y0 = m3*.5/m1;
z0 = m4*.5/m1;
r  = sqrt(x0*x0 + y0*y0 + z0*z0 - m5/m1);

sphere = [x0 y0 z0 r];


function c=dctiii(f,L,dim)
%DCTIII  Discrete Consine Transform type III
%   Usage:  c=dctiii(f);
%           c=dctiii(f,L);
%           c=dctiii(f,[],dim);
%           c=dctiii(f,L,dim);
%
%   `dctiii(f)` computes the discrete cosine transform of type III of the
%   input signal *f*. If *f* is multi-dimensional, the transformation is
%   applied along the first non-singleton dimension.
%
%   `dctiii(f,L)` zero-pads or truncates *f* to length *L* before doing the
%   transformation.
%
%   `dctiii(f,[],dim)` or `dctiii(f,L,dim)` applies the transformation along
%   dimension *dim*.
%
%   The transform is real (output is real if input is real) and orthonormal.
%
%   This is the inverse of |dctii|.
%
%   Let f be a signal of length *L*, let `c=dctiii(f)` and define the vector
%   *w* of length *L* by
%
%   ..  w = [1/sqrt(2) 1 1 1 1 ...]
%
%   .. math:: w\left(n\right)=\begin{cases}\frac{1}{\sqrt{2}} & \text{if }n=0\\1 & \text{otherwise}\end{cases}
%
%   Then
%
%   ..                     L-1
%     c(n+1) = sqrt(2/L) * sum w(m+1)*f(m+1)*cos(pi*(n+.5)*m/L)
%                          m=0
%
%   .. math:: c\left(n+1\right)=\sqrt{\frac{2}{L}}\sum_{m=0}^{L-1}w\left(m\right)f\left(m+1\right)\cos\left(\frac{\pi}{L}\left(n+\frac{1}{2}\right)m\right)
%
%   Examples:
%   ---------
%
%   The following figures show the first 4 basis functions of the DCTIII of
%   length 20:::
%
%     % The dctii is the adjoint of dctiii.
%     F=dctii(eye(20));
%
%     for ii=1:4
%       subplot(4,1,ii);
%       stem(F(:,ii));
%     end;
%
%   See also:  dctii, dctiv, dstii
%
%   References: rayi90 wi94

%   AUTHOR: Peter L. Søndergaard
%   TESTING: TEST_PUREFREQ
%   REFERENCE: REF_DCTIII

complainif_argnonotinrange(nargin,1,3,mfilename);

if nargin<3
  dim=[];
end;

if nargin<2
  L=[];
end;

[f,L,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,L,dim,'DCTIII');

if ~isempty(L)
  f=postpad(f,L);
end;
c=comp_dct(f,3);
% c=zeros(2*L,W,assert_classname(f));
%
% m1=1/sqrt(2)*exp(-(0:L-1)*pi*i/(2*L)).';
% m1(1)=1;
%
% m2=1/sqrt(2)*exp((L-1:-1:1)*pi*i/(2*L)).';
%
% for w=1:W
%   c(:,w)=[m1.*f(:,w);0;m2.*f(L:-1:2,w)];
% end;
%
% c=fft(c)/sqrt(L);
%
% c=c(1:L,:);
%
% if isreal(f)
%   c=real(c);
% end;

c=assert_sigreshape_post(c,dim,permutedsize,order);

% This is a slow, but convenient way of expressing the above algorithm.
%R=1/sqrt(2)*[diag(exp(-(0:L-1)*pi*i/(2*L)));...
%	     zeros(1,L); ...
%	     [zeros(L-1,1),flipud(diag(exp((1:L-1)*pi*i/(2*L))))]];

%R(1,1)=1;

%c=fft(R*f)/sqrt(L);

%c=c(1:L,:);


% Linear Diophantine Equation
%Given three integers a, b, c representing a linear equation of the form : ax + by = c.
%To find if the equation has a solution such that x and y are both integral values.
%Used in programming to find the exact solution exists or not.

function retval = linear_diophantine_eqn (a,b,c)
   if mod(c,gcd(a,b))==0
     retval=true;            % 1 represent yes it exist
   else
     retval= false;          %  0 represent no it doesn't exist

end


clear all, close all, clc

xlim([-2 2])
ylim([-2 2])
zlim([-2 2])
% view([-72 18])
view([45 26])
% axis off

set(gca,'FontSize',13);
set(gcf,'Position',[100 100 300 300])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', 'CH01_EX09_1_axis.eps')

%% Define rotation matrix
t1 = pi/15;
t2 = -pi/9;
t3 = -pi/20;

Rx = [1 0 0;
    0 cos(t1) -sin(t1);
    0 sin(t1) cos(t1)];

Ry = [cos(t2) 0 sin(t2);
    0 1 0;
    -sin(t2) 0 cos(t2)];

Rz = [cos(t3) -sin(t3) 0;
    sin(t3) cos(t3) 0;
    0 0 1];

Sigma = diag([3; 1; 0.5]);

R = Rz*Ry*Rx*Sigma;

%% Plot sphere and great circles
[x,y,z] = sphere(250);
% subplot(1,2,1)
plot3([-2 2],[0 0],[0 0],'k','LineWidth',6);
hold on
plot3([0 0],[-2 2],[0 0],'k','LineWidth',6);
plot3([0 0],[0 0],[-2 2],'k','LineWidth',6);

h1=surf(x,y,z);
hold on
set(h1,'EdgeColor','none','FaceColor',[.5 .5 .5],'FaceAlpha',.75)
% set(h1,'FaceColor','none')
lighting phong
shading interp
axis equal

theta = (0:.001:1)*2*pi;
x1 = cos(theta);
y1 = sin(theta);
z1 = 0*theta;
x2 = 0*theta;
y2 = cos(theta);
z2 = sin(theta);
x3 = cos(theta);
y3 = 0*theta;
z3 = sin(theta);

plot3(x1,y1,z1,'k','LineWidth',6);
plot3(x2,y2,z2,'k','LineWidth',6);
plot3(x3,y3,z3,'k','LineWidth',6);

xlim([-2 2])
ylim([-2 2])
zlim([-2 2])
% view([-72 18])
view([45 26])
% axis off

set(gca,'FontSize',13);
set(gcf,'Position',[100 100 100 100])
set(gcf,'PaperPositionMode','auto')
set(gcf,'renderer','opengl')

axis off
%   save2pdf('CH01_EX09_1b.pdf',gcf,600)
% print('-depsc2', '-loose', 'CH01_EX09_1b.eps')
print('-dpng','-r1800' ,'-loose', 'CH01_EX09_1a.png')

%%
xR = 0*x;
yR = 0*y;
zR = 0*z;

for i=1:size(x,1)
    for j=1:size(x,2)
        vec = [x(i,j); y(i,j); z(i,j)];
        vecR = R*vec;
        xR(i,j) = vecR(1);
        yR(i,j) = vecR(2);
        zR(i,j) = vecR(3);
    end
end

vec1 = [x1; y1; z1];
vec2 = [x2; y2; z2];
vec3 = [x3; y3; z3];

vec1R = R*vec1;
vec2R = R*vec2;
vec3R = R*vec3;

eX = [2; 0; 0];
eY = [0; 2; 0];
eZ = [0; 0; 2];
eXR = R*eX;
eYR = R*eY;
eZR = R*eZ;

x1R = vec1R(1,:);
y1R = vec1R(2,:);
z1R = vec1R(3,:);
x2R = vec2R(1,:);
y2R = vec2R(2,:);
z2R = vec2R(3,:);
x3R = vec3R(1,:);
y3R = vec3R(2,:);
z3R = vec3R(3,:);

figure
% plot3([0 -2],[0 0],[0 0],'Color',[.3 .3 .3],'LineWidth',6);
% hold on
% plot3([0 0],[0 -2],[0 0],'Color',[.3 .3 .3],'LineWidth',6);
% plot3([0 0],[0 0],[0 2],'Color',[.3 .3 .3],'LineWidth',6);
h1=surf(xR,yR,zR,z);
hold on
set(h1,'EdgeColor','none','FaceColor',[.5 .5 .5],'FaceAlpha',.75)
% set(h1,'FaceColor','none')
axis equal
shading interp
lighting phong

plot3(x1R,y1R,z1R,'k','LineWidth',6);
plot3(x2R,y2R,z2R,'k','LineWidth',6);
plot3(x3R,y3R,z3R,'k','LineWidth',6);

plot3([-eXR(1) eXR(1)],[-eXR(2) eXR(2)],[-eXR(3) eXR(3)],'k','LineWidth',6);
plot3([-eYR(1) eYR(1)],[-eYR(2) eYR(2)],[-eYR(3) eYR(3)],'k','LineWidth',6);
plot3([-eZR(1) eZR(1)],[-eZR(2) eZR(2)],[-eZR(3) eZR(3)],'k','LineWidth',6);

xlim([-2 2])
ylim([-2 2])
zlim([-2 2])
grid off

% view([-72 18])
view([45 26])
% axis off

set(gca,'FontSize',13);
set(gcf,'Position',[100 100 100 100])
set(gcf,'PaperPositionMode','auto')
set(gcf,'renderer','opengl')

axis off
%   save2pdf('CH01_EX09_1b.pdf',gcf,600)
% print('-depsc2', '-loose', 'CH01_EX09_1b.eps')
print('-dpng','-r1800' ,'-loose', 'CH01_EX09_1b.png')


function [ x, w ] = moment_method ( n, moment )

%*****************************************************************************80
%
%% MOMENT_METHOD computes a quadrature rule by the method of moments.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub, John Welsch,
%    Calculation of Gaussian Quadrature Rules,
%    Mathematics of Computation,
%    Volume 23, Number 106, April 1969, pages 221-230.
%
%  Parameters:
%
%    Input, integer N, the order of the quadrature rule.
%
%    Input, real MOMENT(2*N+1), moments 0 through 2*N.
%
%    Output, real X(N,1), W(N,1), the points and weights of the quadrature rule.
%
  debug = 0;

  if ( debug )
    r8vec_print ( 2 * n + 1, moment, '  Moments:' );
  end
%
%  Define the N+1 by N+1 Hankel matrix H(I,J) = moment(I+J).
%
  h = zeros ( n + 1, n + 1 );
  for i = 1 : n + 1
    for j = 1 : n + 1
      h(i,j) = moment(i+j-1);
    end
  end

  if ( debug )
    r8mat_print ( n + 1, n + 1, h, '  Hankel matrix:' )
  end
%
%  Compute R, the upper triangular Cholesky factor of H.
%
  r = chol ( h );

  if ( debug )
    r8mat_print ( n + 1, n + 1, r, '  Cholesky factor:' )
  end
%
%  Compute ALPHA and BETA from R, using Golub and Welsch's formula.
%
  alpha = zeros ( n, 1 );
  alpha(1) = r(1,2) / r(1,1);
  for i = 2 : n
    alpha(i) = r(i,i+1) / r(i,i) - r(i-1,i) / r(i-1,i-1);
  end

  beta = zeros ( n - 1, 1 );
  for i = 1 : n - 1
    beta(i) = r(i+1,i+1) / r(i,i);
  end
%
%  Compute the points and weights from the moments.
%
  jacobi = diag ( alpha, 0 ) + diag ( beta, -1 ) + diag ( beta, +1 );

  if ( debug )
    r8mat_print ( n, n, jacobi, '  Jacobi matrix: ' );
  end
%
%  Get the eigendecomposition of the Jacobi matrix.
%
  [ evec, eval ] = eig ( jacobi );

  x = diag ( eval );
  w = moment(1) * evec(1,:).^2;
%
%  Destroy all row vectors.
%
  x = x(:);
  w = w(:);

  return
end


function M=Euler1Ang2RotMat(theta,series,handed)
%%EULER1ANG2ROTMAT  Obtain the Euler-angle rotation matrix for a rotation
%            of angle theta about single axis (x, y, or z). The rotation is
%            either right-handed (counterclockwise, the default) or
%            left-handed depending on handed. The components of vectors to
%            be rotated are assumed ordered [x;y;z].
%
%INPUTS: theta The angle in radians about which the rotation matrix should
%              rotate a vector around the axis given by series. The
%              handedness of the angle is given by handed.
%       series A text string specifying the angle about which the rotation
%              is performed. This can be 'x', 'y', or 'z'.
%       handed The handedness of the rotation angle. If omitted, it is
%              assumed that the rotation is right-handed (the standard).
%              Possible values are:
%              'right' The default if omitted. The rotation is right-
%                      handed.
%              'left'  The rotation is left-handed. The rotation angle is
%                      clockwise when one is looking into the rotation
%                      axis.
%
%OUTPUTS: M The rotation matrix such that M*v rotates a vector v by theta
%           about the specified axis.
%
%Euler angles are discussed in [1].
%
%REFERENCES:
%[1] M. D. Shuster, "A survey of attitude representations," The Journal of
%    Astronautical Sciences, vol. 41, no. 4, pp. 439-517, Oct. - Dec. 1993.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(handed))
   handed='right';
end

switch(handed)
    case 'right'
    case 'left'
        theta=-theta;
    otherwise
        error('Invalid handedness provided.')
end

cosT=cos(theta);
sinT=sin(theta);

switch(series)
    case 'x'
        M=[1, 0,       0;
           0, cosT, -sinT;
           0, sinT,  cosT];
    case 'y'
        M=[cosT, 0,  sinT;
              0, 1,     0;
          -sinT, 0, cosT];
    case 'z'
        M=[cosT, -sinT, 0;
           sinT,  cosT, 0;
              0,    0, 1];
    otherwise
        error('Invalid rotation axis specified')
end

end

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


function [theta, J_history] = gradientDescentMulti(X, y, theta, alpha, num_iters)
%GRADIENTDESCENTMULTI Performs gradient descent to learn theta
%   theta = GRADIENTDESCENTMULTI(x, y, theta, alpha, num_iters) updates theta by
%   taking num_iters gradient steps with learning rate alpha

% Initialize some useful values
m = length(y); % number of training examples
J_history = zeros(num_iters, 1);

for iter = 1:num_iters

    % ====================== YOUR CODE HERE ======================
    % Instructions: Perform a single gradient step on the parameter vector
    %               theta.
    %
    % Hint: While debugging, it can be useful to print out the values
    %       of the cost function (computeCostMulti) and gradient here.
    %

    y_pred = X * theta; % X.shape=(m,3) theta.shape=(3,1)
    theta = theta - alpha * 1/m * (X'*(y_pred-y));  % y.shape=(m,1);

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

    % Save the cost J in every iteration
    J_history(iter) = computeCostMulti(X, y, theta);

end

end


function nksub = ksubset_enum ( k, n )

%*****************************************************************************80
%
%% KSUBSET_ENUM enumerates the K element subsets of an N set.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer K, the number of elements each K subset must
%    have. 0 <= K <= N.
%
%    Input, integer N, the number of elements in the master set.
%    0 <= N.
%
%    Output, integer NKSUB, the number of distinct elements.
%
  nksub = i4_choose ( n, k );

  return
end


%ROT2 SO(2) Rotation matrix
%
% R = ROT2(THETA) is an SO(2) rotation matrix (2x2) representing a rotation of THETA
% radians.
%
% R = ROT2(THETA, 'deg') as above but THETA is in degrees.
%
% See also SE2, TROT2, ISROT2, TRPLOT2, ROTX, ROTY, ROTZ, SO2.

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

function R = rot2(t, deg)

    if nargin > 1 && strcmp(deg, 'deg')
        t = t *pi/180;
    end

    ct = cos(t);
    st = sin(t);
    R = [
        ct  -st
        st   ct
        ];


% Calculate the number of independent cycles (use L = m-n+c)
%      where L = num cycles, m - num edges, n - num nodes,
%                      c - number of connected components
% This is also known as the "cyclomatic number": the number of edges
% that needs to be removed so that the graph doesn't have cycles.
%
% INPUTS: adjacency matrix, nxn
% OUTPUTs: number of independent cycles (or cyclomatic number)
%
% Other routines used: numNodes.m, numEdges.m, findConnComp.m
% GB: last updated, Oct 5 2012

function L = numCycles(adj)

n=numNodes(adj);
m=numEdges(adj);
comp_mat = findConnComp(adj);

L=m-n+length(comp_mat);


function f = flops_lu(n)
% FLOPS_LU   Flops for LU decomposition.
% FLOPS_LU(n) returns the number of flops to compute lu(rand(n,n)).
% The matrix is assumed to be symmetric positive definite, so that no pivoting is required.

% Number of flops for the algorithm in Cormen et al:
% Number of multiplies+adds is:
%   sum(k=1:n) sum(i=k+1:n) sum(j=k+1:n) 3
% = sum(k=1:n) 3(n-k)^2
% = sum(k=0:(n-1)) 3 k^2
% = n(n-0.5)(n-1)
% Number of divides is: n-1
f = n*(n-0.5)*(n-1) + (n-1)*flops_div;