function [yi, p, pval] = newtint(x, y, xi, c)

% NEWTINT Interpolation of equally-spaced points.
%    NEWTINT(X,Y,XI,C) interpolates to find YI, the value
%    of the underlying function Y at the point XI, using
%    either Newton's forward interpolation formula or
%    Newton's backward interpolation formula.
%
%    C specifies the interpolation method:
%       0 : Newton's forward or backward (default)
%       1 : Newton's forward
%       2 : Newton's backward
%       3 : Stirling's method
%       4 : Everett's method
%
%    [YI,P,PVAL] = NEWTINT() also returns P, the coefficients
%    of the calculated interpolating polynomial, and PVAL,
%    which specifies the maximum degree of the interpolating
%    polynomial.

% Joe Henning - Fall 2011

if (nargin < 4)
   c = 0;
end

pval = length(x)*(0+1) - 1;

n = length(x)-1;
D = zeros(n+1,n+1);
D(:,1) = y(:);

h = x(2)-x(1);

for i = 1:n
   for j = 1:i
      D(i+1,j+1) = D(i+1,j)-D(i,j);
   end
end

if (c == 0)
   if (xi < x(1))
      c = 2;
   elseif (xi > x(ceil((n+1)/2)) && xi <= x(n+1))
      c = 2;
   else
      c = 1;
   end
end

switch c
   case 1
      % forward difference interpolation
      p = diag(D);
      q = (xi-x(1))/h;
      yi = p(1);
      for i = 1:length(p)-1
         term = 1;
         for k = 0:(i-1)
           term = term*(q-k);
         end
         term = term/factorial(i);
         yi = yi + term*p(i+1);
      end
   case 2
      % backward difference interpolation
      p = D(n+1,:).';
      q = (xi-x(n+1))/h;
      yi = p(1);
      for i = 1:length(p)-1
         term = 1;
         for k = 0:(i-1)
           term = term*(q+k);
         end
         term = term/factorial(i);
         yi = yi + term*p(i+1);
      end
   case 3
      % Stirling's method
      xk = 0;
      ik = 0;
      for i = 1:n+1
         if (xi > x(i))
            xk = x(i);
            ik = i;
         end
      end
      yi = 0;
      q = (xi-xk)/h;
      if (q > 0.5)
         ik = ik + 1;
         xk = x(ik);
         q = (xi-xk)/h;
         for i = 1:2:n+1
            fi = floor(i/2);
            term = 0;
            %[term D(ik+i-1-floor(i/2),i)]s
            for k = fi-1:fi
               term = term + dnchoosek(q+k,i-1)/2.0;
            end
            p(i,1) = term;
            if (ik+i-1-floor(i/2) <= n+1)
               yi = yi + term*D(ik+i-1-floor(i/2),i);
            end
         end
         for i = 2:2:n+1
            fi = floor(i/2);
            term = dnchoosek(q+fi-1,i-1);
            p(i,1) = term;
            %[term D(ik+i-1-floor(i/2),i) D(ik+i-floor(i/2),i)]
            if (ik+i-floor(i/2) > n+1)
               yi = yi + term*D(ik+i-1-floor(i/2),i)/2.0;
            else
               yi = yi + term*(D(ik+i-1-floor(i/2),i)+D(ik+i-floor(i/2),i))/2.0;
            end
         end
      else
         for i = 1:2:n+1
            fi = floor(i/2);
            term = 0;
            for k = fi-1:fi
               term = term + dnchoosek(q+k,i-1)/2.0;
            end
            p(i,1) = term;
            %[term D(ik+i-1-floor(i/2),i)]
            if (ik+i-1-floor(i/2) <= n+1)
               yi = yi + term*D(ik+i-1-floor(i/2),i);
            end
         end
         for i = 2:2:n+1
            fi = floor(i/2);
            term = dnchoosek(q+fi-1,i-1);
            p(i,1) = term;
            %[term D(ik+i-1-floor(i/2),i) D(ik+i-floor(i/2),i)]
            if (ik+i-floor(i/2) > n+1)
               yi = yi + term*D(ik+i-1-floor(i/2),i)/2.0;
            else
               yi = yi + term*(D(ik+i-1-floor(i/2),i)+D(ik+i-floor(i/2),i))/2.0;
            end
         end
      end
   case 4
      % Everett's method
      xk = 0;
      ik = 0;
      for i = 1:n+1
         if (xi > x(i))
            xk = x(i);
            ik = i;
         end
      end
      p = [];
      q = (xi-xk)/h;
      r = 1-q;
      yi = 0;
      for i = 1:2:n+1
         fi = floor(i/2);
         e = dnchoosek(r+fi,i);
         f = dnchoosek(q+fi,i);
         p = [p; e f];
         %[e D(ik+i-1-floor(i/2),i) f D(ik+i-floor(i/2),i)]
         if ((ik+i-floor(i/2)) > n+1)
            yi = yi + e*D(ik+i-1-floor(i/2),i) + f*0;
         else
            yi = yi + e*D(ik+i-1-floor(i/2),i) + f*D(ik+i-floor(i/2),i);
         end
      end
end

function [d] = dnchoosek(n, k)

% DNCHOOSEK Binomial coefficient or all combinations.
%    DNCHOOSEK(N,K) where N and K are non-negative numbers
%    returns N!/K!(N-K)!.

d = gamma(n+1)/(gamma(k+1)*gamma(n-k+1));


function h = ksizeROT(npd,noIQR)
% "Rule of Thumb" estimate (Silverman)
%    Estimate is based on assumptions of Gaussian data and kernel
%    Actually the multivariate version in Scott ('92)
%  Use ksizeROT(X,1) to force use of stddev. instead of min(std,C*iqr)
%       (iqr = interquartile range, C*iqr = robust stddev estimate)
%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

  X = getPoints(npd);
  N = size(X,2);  dim = size(X,1);
  if (nargin<2) noIQR=0; end;

  Rg = .282095; Mg=1;                     % See ksizeCalcUseful for derivation
  Re = .6;      Me = .199994;             %   this is the canonical kernel adjustment
  Rl = .25;     Ml = 1.994473;            %   for product kernels of these types
  switch(npd.type),
      case 0, prop = 1.0;                 % Approximate; 1D prop = 1.059224; % Gaussian
      case 1, prop = ((Re/Rg)^dim / (Me/Mg)^2 )^(1/(dim+4)); % 1D prop = 2.344944; % Epanetchnikov
      case 2, prop = ((Rl/Rg)^dim / (Ml/Mg)^2 )^(1/(dim+4)); % 1D prop = 0.784452; % Laplacian
  end;

  sig = std(X,0,2);            % estimate sigma (standard)
  if (noIQR)
    h = prop*sig*N^(-1/(4+dim));
  else
    iqrSig = .7413*iqr(X')';     % find interquartile range sigma est.
    if (max(iqrSig)==0) iqrSig=sig; end;
    h = prop * min(sig,iqrSig) * N^(-1/(4+dim));
  end;

%  if (min(h) == 0) warning('Near-zero covariance => Kernel size set to 0'); end;


function [y,t] = ramp(A,T,t0,c)
%
%RAMP unit ramp function
% function [y,t] = ramp(A,T,t0,c)
% This function generates the unit ramp function of
%amplitude A
%width 2T.
%shift of t0
%Scaling factor of c
%If no output is mentioned the function plots the signal.
%
%Example:
%ramp(1,2,3,4) plots the rampl signal of amplitude 1, stimewidth of 4
%time-shift of 3 and scaling factor of 4.
%[y,t]=ramp(1,2,3,4) is same as above with no plot.
%
% Author: B.H.Sri Hari, IITMadras
% Copyright B.H.Sri Hari.
%$Date:03/28/2006 13:04:15$
if nargin < 3,
    t0 = 0;
    c=1;
elseif nargin==3,
    c=1;
end
ra = abs(t0/c);
if ra < 1 | ra==0
    t = -2*T:.01:2*T;
elseif ra>=1 | ra==0
    t = -2*T*(ra):.01:2*T*(ra);
end
y = A*(c*t-t0).*(c*t>=t0);
y1 = zeros(1,length(t));
if nargout == 0,
    plot(t,y,'k','LineWidth',2);
    hold on;
    plot(t,y1,':k');
    hold on;
    plot(y1,t,':k')
    hold off
    title('Ramp signal')
   if A<0 & ra ~= 0
        axis([-2*T*(ra) 2*T*(ra) 4*A/3 -A/4]);
    elseif A<0  & ra == 0
        axis([-2*T 2*T 4*A/3 -A/4])
    elseif A>0 & ra~=0
        axis([-2*T*(ra) 2*T*(ra) -A/4 4*A/3]);
    else
            axis([-2*T 2*T -A/4 4*A/3]);
    end
end


function d = divisor(n)
%% divisor : provides a list of integer divisors of a number.
% divisor(n) : row vector of all distinct divisors of a positive integer N,
%               including 1 and N.
%
% Remark:
%   This function uses the default factor() routine in Matlab and hence is
% limited to input values upto 2^32. However if factor() routine does get
% updated for larger integers, this function will still work fine.
%   Using factor() provides a significant speed improvement over manually
% seaching for the each divisor of n.
%
% Example:
%   a = divisor(12);
%   returns -> a = [1, 2, 3, 4, 6, 12];
%
% See Also:
%   factor, primes

% Author: Yash Kochar ( yashkochar@yahoo.com )
% Last modified: 21st June 2009
%-------------------------------------------------------------------------------

%% Input error check :
%   Check whether input is positive integer and scalar.
if ~isscalar(n)
    error('divisor:NonScalarInput','Input must be a scalar.');
end
if (n < 1) || (floor(n) ~= n)
  error('divisor:PositiveIntegerOnly', 'Input must be a positive integer.');
end

%% Find prime factors of number :
pf = factor(n);         % Prime factors of n
upf = unique(pf);       % Unique

%% Calculate the divisors
d = upf(1).^(0:1:sum(pf == upf(1)))';
for f = upf(2:end)
    d = d*(f.^(0:1:sum(pf == f)));
    d = d(:);
end
d = sort(d)';   % To further improve the speed one may remove this sort command
                %   Just remember to take the transpose of "d" to get a result
                %   as a row vector instead of a column vector.


function out = mapFeature(X1, X2)
% MAPFEATURE Feature mapping function to polynomial features
%
%   MAPFEATURE(X1, X2) maps the two input features
%   to quadratic features used in the regularization exercise.
%
%   Returns a new feature array with more features, comprising of
%   X1, X2, X1.^2, X2.^2, X1*X2, X1*X2.^2, etc..
%
%   Inputs X1, X2 must be the same size
%

degree = 6;
out = ones(size(X1(:,1)));
for i = 1:degree
    for j = 0:i
        out(:, end+1) = (X1.^(i-j)).*(X2.^j);
    end
end

end


function x = cc_abscissas ( n )

%*****************************************************************************80
%
%% CC_ABSCISSAS computes the Clenshaw Curtis abscissas.
%
%  Discussion:
%
%    The interval is [ -1, 1 ].
%
%    The abscissas are the cosines of equally spaced angles between
%    180 and 0 degrees, including the endpoints.
%
%      X(I) = cos ( ( ORDER - I ) * PI / ( ORDER - 1 ) )
%
%    except for the basic case ORDER = 1, when
%
%      X(1) = 0.
%
%    If the value of ORDER is increased in a sensible way, then
%    the new set of abscissas will include the old ones.  One such
%    sequence would be ORDER(K) = 2*K+1 for K = 0, 1, 2, ...
%
%    When doing interpolation with Lagrange polynomials, the Clenshaw Curtis
%    abscissas can be a better choice than regularly spaced abscissas.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2012
%
%  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.
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%    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
  end

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

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

  return
end


function f = rhs ( node_num, node_xy, time )

%*****************************************************************************80
%
%% RHS gives the right-hand side of the differential equation.
%
%  Discussion:
%
%    We assume that the equation to be solved is
%
%      dUdT - Laplacian U + K * U = F
%
%    with
%
%      K = 0,
%
%    and
%
%      F = (2*pi*pi-1)*sin(pi*x)*sin(pi*y)*exp(-t).
%
%    The exact solution is:
%
%      U = sin(pi*x) * sin(pi*y) * exp(-t)
%
%  Modified:
%
%    07 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of points.
%
%    Input, real NODE_XY(2,NODE_NUM), the coordinates of the points.
%
%    Input, real TIME, the current time (assumed to be
%    the initial time).
%
%    Output, real F(NODE_NUM), the value of the right hand side.
%
  f(1:node_num) = ( 2.0 * pi * pi - 1.0 ) ...
     * sin ( pi * node_xy(1,1:node_num) ) ...
    .* sin ( pi * node_xy(2,1:node_num) ) * exp ( - time );

  return
end


%
% [K, dK] = gpK_decper(D, p1, p2, p3, p4)
%
% Decaying periodic covariance function for GP.
%
% p1 is the scale
% p2 is the decaying length
% p3 is the frequency of the periodicity
% p4 is the smoothness of the periodic signal
%
function [K, dK] = gpK_decper(D, p1, p2, p3, p4)
K = p1^2*exp(-0.5*(D.^2)/(p2^2)-2*sin(pi*D*p3).^2/(p4^2));

if nargout >= 2
  dK = zeros([size(D),4]);
  dK(:,:,1) = K .* 2 / p1;
  dK(:,:,2) = K .* (-0.5*D.^2) .* (-2*p2^(-3));
  dK(:,:,3) = K .* (-2*sin(pi*D*p3)*2) .* cos(pi*D*p3) .* (pi*D);
  dK(:,:,4) = K .* (-2*sin(pi*D*p3).^2) * (-2)*p4^(-3);
end


function y = logMvGamma( x, D)
% Compute logarithm multivariate Gamma function.
%   operates on each entry of input matrix/vector x
%INPUT
%   x : any scalar/vector/matrix
%   D : integer dimension of the mv Gamma function
%MATH DETAILS -------------------------------------------------------
% Gamma_D(x) = pi^(D(D-1)/4) prod_(j=1)^p Gamma(x+(1-j)/2)
% log Gamma_D(x) = D(D-1)/4 log pi + sum_(j=1)^p log Gamma(x+(1-j)/2)
% Credit: Michael Chen (sth4nth@gmail.com).

s = size(x);
ds = (1-(1:D)')/2;

% Force input x to be a row vector
X = reshape(x,1,prod(s));

% X(dd, dim) = input(dim) - ds(dd)
X = bsxfun(@plus, X, ds   );

y = D*(D-1)/4*log(pi) + sum(gammaln(X),1);

% Force output back to size of original input
y = reshape(y,s);


%%  Gaussian mask
%   k: the scaling factor
%   s: standard variance
%   M: mask

function M = gaussianMask(k,s)
M=[];
if s==0
    M=1;
    return;
end
R = ceil(3*s); % cutoff radius of the gaussian kernal
for i = -R:R,
    for j = -R:R,
        M(i+R+1,j+R+1) = k * exp(-(i*i+j*j)/2/s/s)/(2*pi*s*s);
    end
end


function pwl_interp_2d_scattered_test03 ( prob )

%*****************************************************************************80
%
%% PWL_INTERP_2D_SCATTERED_TEST03 tests PWL_INTERP_2D_SCATTERED_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 October 2012
%
%  Author:
%
%    John Burkardt
%
  ni = 25;
  g = 2;
  nd = g00_size ( g );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PWL_INTERP_2D_SCATTERED_TEST03\n' );
  fprintf ( 1, '  PWL_INTERP_2D_SCATTERED_VALUE evaluates a\n' );
  fprintf ( 1, '  piecewise linear interpolant to scattered data.\n' );
  fprintf ( 1, '  Here, we use grid number %d\n', g );
  fprintf ( 1, '  with %d scattered points in the unit square\n', nd );
  fprintf ( 1, '  on problem %d\n', prob );
%
%  Get the data points and evaluate the function there.
%
  [ xd, yd ] = g00_xy ( g, nd );

  zd = f00_f0 ( prob, nd, xd, yd );

  xyd(1,1:nd) = xd(1:nd);
  xyd(2,1:nd) = yd(1:nd);
%
%  Set up the Delaunay triangulation.
%
  [ element_num, triangle, element_neighbor ] = r8tris2 ( nd, xyd );
%
%  Define the interpolation points.
%
  k = 0;
  for i = 1 : 5
    for j = 1 : 5
      k = k + 1;
      xyi(1,k) = ( 2 * i - 1 ) / 10.0;
      xyi(2,k) = ( 2 * j - 1 ) / 10.0;
    end
  end

  xi(1:ni,1) = xyi(1,1:ni);
  yi(1:ni,1) = xyi(2,1:ni);

  ze = f00_f0 ( prob, ni, xi, yi );
%
%  Evaluate the interpolant.
%
  zi = pwl_interp_2d_scattered_value ( nd, xyd, zd, element_num, ...
    triangle, element_neighbor, ni, xyi );

  rms = 0.0;
  for k = 1 : ni
    rms = rms + ( zi(k) - ze(k) )^2;
  end
  rms = sqrt ( rms / ni );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  RMS error is %g\n', rms );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     K      Xi(K)       Yi(K)       Zi(K)       Z(X,Y)\n' );
  fprintf ( 1, '\n' );

  for k = 1 : ni
    fprintf ( 1, '  %4d  %10f  %10f  %10f  %10f\n', ...
      k, xyi(1,k), xyi(2,k), zi(k), ze(k) );
  end

  return
end


function [Feven,Fodd] = filterGabor2d( r, sig, lam, theta, omega, show )
% Creates an even/odd pair of 2D Gabor filters.
%
% Creates a pair of Gabor filters (one odd one even) at the specified
% orientation. For Thomas' ECCV98 filters, use sig=sqrt(2), lam=4.  Note
% that originally this function computed a quadratic masked with a
% Gaussian, and not a sin/cos masked with a Gaussian. Requires Matlab's
% 'Signal Processing Toolbox'.
%
% USAGE
%  [Feven,Fodd] = filterGabor2d( r, sig, lam, theta, [omega], [show] )
%
% INPUTS
%  r       - final mask will be 2r+1 x 2r+1
%  sig     - standard deviation of Gaussian mask
%  lam     - elongation of Gaussian mask
%  theta   - orientation (in degrees)
%  omega   - [1] wavlength of underlying sine (sould be >=1)
%  show    - [0] figure to use for optional display
%
% OUTPUTS
%  Feven   - even symmetric filter (-cosine masked with Gaussian)
%  Fodd    - odd symmetric filter (-sine masked with Gaussian)
%
% EXAMPLE
%  [Feven,Fodd] = filterGabor2d(15,sqrt(2),4,45,1,1);
%  [Feven,Fodd] = filterGabor2d(25,4,2,0,2,3);
%
% See also FILTERGABOR1D, FILTERGAUSS
%
% Piotr's Image&Video Toolbox      Version 2.12
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<5 || isempty(omega) ); omega=1; end
if( nargin<6 || isempty(show) ); show=0; end

% create even/odd Gabor filters
[x,y]=meshgrid(-r:r,-r:r);
mask = exp(-(y.^2)/(sig^2)-(x.^2)/(lam^2*sig^2));
Feven = -cos(2*pi*y/4/omega) .* mask;
%Feven = (4*(y.^2)/(sig^4)-2/(sig^2)).*mask; % original function was y.^2
Fodd = imag(hilbert(Feven));

% orient appropriately
Feven = imrotate(Feven,theta,'bil','crop');
Fodd = imrotate(Fodd,theta,'bil','crop');

% Set mean to 0 (should already be 0)
Feven=Feven-mean(Feven(:));
Fodd=Fodd-mean(Fodd(:));

% set L1norm to 0
Feven=Feven/norm(Feven(:),1);
Fodd=Fodd/norm(Fodd(:),1);

% display
if( show )
  filterVisualize( Feven, show );
  filterVisualize( Fodd, show+1 );
end


%RPY2TR Roll-pitch-yaw angles to SE(3) homogeneous transform
%
% T = RPY2TR(ROLL, PITCH, YAW, OPTIONS) is an SE(3) homogeneous
% transformation matrix (4x4) with zero translation and rotation equivalent
% to the specified roll, pitch, yaw angles angles. These correspond to
% rotations about the Z, Y, X axes respectively. If ROLL, PITCH, YAW are
% column vectors (Nx1) then they are assumed to represent a trajectory and
% R is a three-dimensional matrix (4x4xN), where the last index corresponds
% to rows of ROLL, PITCH, YAW.
%
% T = RPY2TR(RPY, OPTIONS) as above but the roll, pitch, yaw angles are
% taken from the vector (1x3) RPY=[ROLL,PITCH,YAW]. If RPY is a matrix
% (Nx3) then R is a three-dimensional matrix (4x4xN), where the last index
% corresponds to rows of RPY which are assumed to be
% ROLL,PITCH,YAW].
%
% Options::
%  'deg'      Compute angles in degrees (radians default)
%  'xyz'      Rotations about X, Y, Z axes (for a robot gripper)
%  'zyx'      Rotations about Z, Y, X axes (for a mobile robot, default)
%  'yxz'      Rotations about Y, X, Z axes (for a camera)
%  'arm'      Rotations about X, Y, Z axes (for a robot arm)
%  'vehicle'  Rotations about Z, Y, X axes (for a mobile robot)
%  'camera'   Rotations about Y, X, Z axes (for a camera)
%
% Note::
% - Toolbox rel 8-9 has the reverse angle sequence as default.
% - ZYX order is appropriate for vehicles with direction of travel in the X
%   direction.  XYZ order is appropriate if direction of travel is in the Z
%   direction.
% - 'arm', 'vehicle', 'camera' are synonyms for 'xyz', 'zyx' and 'yxz'
%   respectively.
%
% See also TR2RPY, RPY2R, EUL2TR.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function T = rpy2tr(roll, varargin)

    R = rpy2r(roll, varargin{:});
    T = r2t(R);


function [mu, Q] = matrixCoherence(A)
% Computes the coherence of a matrix `A`, that is
% the maximum absolute value of the cross correlations
% between the columns of `A`
%
% USAGE:
%
%    [mu, Q] = matrixCoherence(A)
%
% INPUT:
%    A:     Matrix
%
% OUTPUTS:
%    mu:    coherence
%    Q:     matrix used by `mu`

n = size(A, 2);
Q = zeros(n, n);
for j = 1:n
    for k = 1:n
        if j ~= k
            Q(j, k) = abs(A(:, j)' * A(:, k)) / (norm(A(:, j)) * norm(A(:, k)));
        end
    end
end
mu = max(max(Q));


function S=hypergeometric0F1(a,z)
%%HYPERGEOMETRIC0F1 Evaluate the confluent hypergeometric function 0F1 for
%                   real or complex parameters. This is the confluent
%                   hypergeometric limit function 0F1(;a;z).
%
%INPUTS: a A real or complex scalar value.
%        z A real or complex scalar value.
%
%OUTPUTS: S The value 0F1(;a;z).
%
%This implements the Taylor series expansion of APpendix I of [1]. It was
%noted that the series generated at least 12 digits of accuracy for
%abs(z)<=1000. A check was added for values of a that are real and negtive
%as the solution should be infinite in that case. However, neighboring
%solutions are finite.
%
%REFERENCES:
%[1] J. Pearson, "Computation of hypergeometric functions," Master's
%    thesis, University of Oxford, Worcester College, 4 Sep. 2009.
%
%July 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Check for the special case.
if(isreal(a)&&a<0)
   S=Inf;
   return;
end

A=1;
S=A;

epsVal=Inf;
k=0;
while(epsVal>eps(S))
    A=A*(1/(a+k))*(z/(k+1));
    S=S+A;

    epsVal=abs(A);

    k=k+1;
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 OutMatrix = IncompleteBellPoly(Nin,Kin,DataList)
%INCOMPLETEBELLPOLY  An iterative method for computing the incomplete (also known as the second kind of) Bell polynomials.
%
%   Purpose:
%   Given a list of input values (x_{1},x_{2},...,x_{N}), the script returns a matrix of Bell polynomials B_{n,k} for
%   n=0,...,N and k=0,...,K.
%
%   Syntax:
%   OutMatrix = IncompleteBellPoly(Nin,Kin,DataList)
%   where
%   B_{n,k} = OutMatrix(n+1,k+1)
%   n=0,...,Nin
%   k=0,...,Kin (Kin<=Nin)
%   DataList = (x_{1},x_{2},...,x_{Nin})
%
%   Authors:
%   Patrick Kano and Moysey Brio
%   University of Arizona
%
%   Email:
%   brio@math.arizona.edu
%
%   Latest Modification Date:
%   April 3, 2007
%
%   Discussion:
%   Given Taylor expansion coefficients of a function g(t) {g_{0},g_{1},g_{2},...} with g_{0}=0,
%   B_{n,k}(g_{0},g_{1},...,g_{n-k+1}) is the nth Taylor coefficient of the kth derivative of g(t)/(k!) in terms of {g_{0},g_{1},g_{2},...}
%   \frac{1}{k!} g^{k}(t) = \sum_{n=0}^{\infty} B_{n,k} \frac{t^{n}}{n!}
%
%   The Bell polynomials can be computed efficiently by a recursion relation
%   B_{n,k} = \sum_{m=1}^{n-k+1} \binom{n-1}{m-1} g_{m} B_{n-m,k-1}
%   where
%   B_{0,0} = 1;
%   B_{n,0} = 0; for n=>1
%   B_{0,k} = 0; for k=>1
%
%   The coefficients can be stored in a lower triangular matrix.  The elements of the kth column are the Taylor coefficients
%   of the kth derivative of g(t)/k!.
%
%   In application, the polynomials arise in multiple contexts in combinatorics.  They also can be found in Riordan's
%   formulation of di Bruno's formula for computing an arbitrary order derivative of the composition of two functions
%   \frac{d^{n}}{dt^{n}} g(f(t)) = \sum_{k=0}^{n} g^{k}(f(t)) B_{n,k}(f^{1}(t),f^{2}(t),...,f^{n-k+1}(t))
%
%   Script Check:
%   If DataList=1 for all entries, B_{n,k} = S(n,k) = Stirling number of the second kind for (n,k)
%
%   Failure Return:
%   OutMatrix is undefined if the code fails.  An error statement is issued and the function exits.
%
%   References:
%   Ferrell S. Wheeler, Bell Polynomials, ACM SIGSAM Bulletin, vol. 21, issue 3, pp.44-53, 1987.
%
%   Warren P. Johnson, The curious history of Faa di Bruno's formula, The American mathematical monthly, vol. 109, no. 3, pp. 217-234, March 2002.
%
%   http://en.wikipedia.org/wiki/Bell_polynomials
%
%   Supported by: Grant # NSF ITR-0325097

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%n - row index
%k - column index

%Check that sufficient input variables x_{1},x_{2},...,x_{N} have been given.
%Note: To compute B_{n,k} requires (x_{1},x_{2},...,x_{n-k+1}) input terms.
%However, we want B_{n=1...N,k=1...N), which includes B_{n=N,k=1}.
%To determine B_{N,1}, we thus require (x_{1},x_{2},...,x_{N}) input terms.

ListLength = length(DataList);

if(ListLength<Nin)
 ErrorStatement = sprintf('Insufficient number of user defined input values %d.  Computation requires %d.  Exiting...\n',ListLength,Nin);
 error(ErrorStatement);

elseif(Nin<0 || Kin<0)
 ErrorStatement = sprintf('N and K must be non-negative.  Exiting... \n');
 error(ErrorStatement);

elseif(Nin<Kin)
 ErrorStatement = sprintf('K must be less than or equal to N.  Exiting... \n');
 error(ErrorStatement);

elseif(Nin==0 && Kin==0)
 OutMatrix = [1];
 return;

elseif(Nin>0 && Kin==0)
 OutMatrix = zeros(Nin+1,1);
 OutMatrix(1,1) = 1;
 return;

else
 %Compute the recursion relationship
 %Note: To keep with Matlab's convention for starting indices at 1 we compute only the non-trivial elements of the matrix.

 %Initialize the Triangular Matrix
 Bm = zeros(Nin,Kin);

 %Starting values for the recurrence relations
 %Bm(0,0) = 1;
 %Bm(n,0) = 0; for n=>1
 %Bm(0,k) = 0; for k=>1
 %kidx=1
 Bm(1:Nin,1) = DataList(1:Nin);

 %Generate each required row of data
 for(nidx=2:Nin) %row, Note: Row 1 has only 1 non-zero element

  for(kidx=2:Kin) %column, Note: Column 1 is the given input array

   for(midx=1:nidx-kidx+1) %recursion
    Bm(nidx,kidx) = Bm(nidx,kidx) + nchoosek(nidx-1,midx-1)*DataList(midx)*Bm(nidx-midx,kidx-1);
   end

  end
 end

 %Append [1,0,...] to generate the complete output matrix
 OutMatrix = eye(Nin+1,Kin+1);
 OutMatrix(2:Nin+1,2:Kin+1) = Bm;
end

end %End of the Function


% GENERATE VARIOUS TIME AND FREQUENCY-VARYING TEST SIGNALS
%
% Three types of signals can be generated:
%
%   1. Deterministic signals (s(t))
%   2. Noisy signals (n(t))
%   3. Deterministic signals in additive noise (s(t) + n(t))
%   4. Analytic signals
%   4. Demo signals
%
%
% 1. The type of deterministic signals s(t) that can be generated are:
%
%   (i)   Linear Frequency Modulated (FM)
%   (ii)  Quadratic FM
%   (iii) Cubic FM
%   (iv)  Stepped FM
%   (v)   Sinusoidal FM
%   (vi)  Hyperbolic FM
%
%   The sampling frequency and length of the signal can be specified.
%   Also the frequency bounds (starting and ending frequency) can be
%   specified.
%
% 2. Noise types n(t) include:
%
%    (i) Uniformly distributed random variable
%    (ii) Gaussian distributed random variable
%
% 3. The signal s(t) can be added to noise n(t).
%
% 4. An analytic associate (complex) can be generated from a real
%    input time domain signal.
%
% 5. Various types of demonstration signals can be loaded.
%
%
%
%  See Also:  gsig, analyt


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

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

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

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost of a particular choice of theta.
%               You should set J to the cost.
%               Compute the partial derivatives and set grad to the partial
%               derivatives of the cost w.r.t. each parameter in theta
%
% Note: grad should have the same dimensions as theta
%

predictions =  sigmoid(X*theta);

leftPart = -y' * log(predictions);

rightPart = (1 - y') * log(1 - predictions);

J = (1 / m) * (leftPart - rightPart);

grad = (1 / m) * ((predictions - y)' * X);

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

end


function [x,y,z]=ell2xyz(lat,lon,h,a,e2)
% ELL2XYZ  Converts ellipsoidal coordinates to cartesian.
%   Vectorized.
% Version: 2011-02-19
% Useage:  [x,y,z]=ell2xyz(lat,lon,h,a,e2)
%          [x,y,z]=ell2xyz(lat,lon,h)
% Input:   lat - vector of ellipsoidal latitudes (radians)
%          lon - vector of ellipsoidal E longitudes (radians)
%          h   - vector of ellipsoidal heights (m)
%          a   - ref. ellipsoid major semi-axis (m); default GRS80
%          e2  - ref. ellipsoid eccentricity squared; default GRS80
% Output:  x \
%          y  > vectors of cartesian coordinates in CT system (m)
%          z /

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin ~= 3 & nargin ~= 5
  warning('Incorrect number of input arguments');
  return
end
if nargin == 3
  [a,b,e2]=refell('grs80');
end

v=a./sqrt(1-e2*sin(lat).*sin(lat));
x=(v+h).*cos(lat).*cos(lon);
y=(v+h).*cos(lat).*sin(lon);
z=(v.*(1-e2)+h).*sin(lat);


function program_02 ( )

%*****************************************************************************80
%
%% PROGRAM_02 demonstrates point and triangle orientation.
%
%  Discussion:
%
%    The program
%    * reads a triangle T (defined by three points),
%    * determines the orientation of the triangle;
%    * computes and prints the length of the edges;
%    * reads a point P
%    * computes the signed distance from P to each line
%      (not the line segment!)
%    * reports whether the point is INSIDE, ON or OUTSIDE the triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_02 - Point/Triangle Orientation\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Define a triangle T:\n' );

  t_v1 = input ( '  Enter [ T.v1.x, T.v1.y]: ' );
  t_v2 = input ( '  Enter [ T.v2.x, T.v2.y]: ' );
  t_v3 = input ( '  Enter [ T.v3.x, T.v3.y]: ' );
%
%  Compute the edges.
%
  t_e1 = t_v2 - t_v1;
  t_e2 = t_v3 - t_v2;
  t_e3 = t_v1 - t_v3;
%
%  Compute the unit normal vectors to each edge.
%  MATLAB's NORM function returns the square root of the sum
%  of the squares of the entries of a vector.
%
  t_e1_nv = [ -t_e1(2), t_e1(1) ] / norm ( t_e1 );
  t_e2_nv = [ -t_e2(2), t_e2(1) ] / norm ( t_e2 );
  t_e3_nv = [ -t_e3(2), t_e3(1) ] / norm ( t_e3 );
%
%  Orientation test.
%  The signed distance of V3 from the line from V1 to V2 must be positive.
%
  test = t_e1_nv * ( t_v3 - t_v1 )';

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Orientation test value = %f\n', test );

  if ( test < 0.0 )
    fprintf ( 1, '  Error!  The triangle is improperly oriented.\n' );
    return
  elseif ( test == 0.0 )
    fprintf ( 1, '  Error!  The triangle is degenerate!\n' );
  else
    fprintf ( 1, '  The triangle is properly oriented.\n' );
  end
%
%  Edge lengths.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Edge1 = v2-v1, length = %f\n', norm ( t_e1 ) );
  fprintf ( 1, '  Edge2 = v3-v2, length = %f\n', norm ( t_e2 ) );
  fprintf ( 1, '  Edge3 = v1-v3, length = %f\n', norm ( t_e3 ) );
%
%  Check various points.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now we see whether a point is in the triangle.\n' );
  fprintf ( 1, '\n' );

  while ( 1 )

    p = [];
    fprintf ( 1, '\n' );
    p = input ( '  Enter a point [ P.x, P.y], or RETURN to quit: ' );

    if ( isempty ( p ) )
      break
    end

%  Compute the signed distance from the point to the line that
%  includes each edge.
%
    p_e1_dist = t_e1_nv * ( p - t_v1 )';
    p_e2_dist = t_e2_nv * ( p - t_v2 )';
    p_e3_dist = t_e3_nv * ( p - t_v3 )';

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Distance to line through edge 1 = %f\n', p_e1_dist );
    fprintf ( 1, '  Distance to line through edge 2 = %f\n', p_e2_dist );
    fprintf ( 1, '  Distance to line through edge 3 = %f\n', p_e3_dist );

    fprintf ( 1, '\n' );
    if ( p_e1_dist < 0.0 | p_e2_dist < 0.0 | p_e3_dist < 0.0 )
      fprintf ( 1, '  The point is OUTSIDE the triangle\n' );
    elseif ( p_e1_dist == 0.0 | p_e2_dist == 0.0 | p_e3_dist == 0.0 )
      fprintf ( 1, '  The point is ON the triangle\n' );
    else
      fprintf ( 1, '  The point is INSIDE the triangle.\n' );
    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_02\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


function [xvec, yvec]=generatepointsregular(npoints,diamcirc,origin,phaseoffset)
% [xvec, yvec]=generatepointsregular(npoints,diamcirc,origin,pahseoffset)
% generates points qually distributed around the circle with 2r=diamcirc
% with center at origin
% phaseoffset: offster of the phase of the first point (default == 0);
if ~exist('phaseoffset','var') %to make oblique configuration
    phaseoffset = 0;
end
angstep=2*pi/npoints;
angvectmp=(0:angstep:2*pi)+phaseoffset;
angvec=angvectmp(1:end-1); %last opint = first point
xvec=(origin(1)+0.5*diamcirc*cos(angvec))';
yvec=(origin(2)+0.5*diamcirc*sin(angvec))';


function [H,mu,SR]=standardGaussKernelBW(param1,method,spherData,N)
%%STANDARDGAUSSKERNELBW Estimate the (univariate or multivariate) kernel
%             bandwidth when approximating a PDF using a set of samples
%             with a Gaussian kernel. This function provides the bandwidth
%             estimate based on the optimal solution if the underlying
%             density of the samples is normal. If the density is not
%             normal, then this function is just an approximation. The
%             kernel bandwidth estimate can be used with the points xi in
%             the function kernelApprox to approximate the continuous
%             density.
%
%INPUTS: param1 If N is an empty matrix or is omitted, then this is:
%              xi A numDimXnumSamples set of samples of the distribution
%                 from which a Gaussian kernel estimator should interpolate
%                 a PDF. For multidimensional estimation, there must be
%                 >=numDim samples. Specifically, the sample covariance
%                 matrix must be positive definite.
%           If N is given, then this is:
%              SR A root covariance matrix such that SR*SR' equals the
%                 (exact or approximated) covariance matrix of the PDF
%                 being approximated.
%    method An optional parameter specifying the method that should be used
%           to estimate the bandwidth. Possible values are
%           0 Use the method implied by Equation 3.30 and 3.28 in [1],
%             which uses the lesser of the sample standard deviation or a
%             scaled interquartile range. This approach is supposed to work
%             well when the underlying distribution of the samples is not
%             actually normal and might be bimodal. This method cannot be
%             used if N is given.
%           1 (The default if omitted or an empty matrix is passed) Use the
%             method of Equation 3.28 in [1], based on the sample standard
%             deviation.
%           2 Use the method of Equation 3.29 in [1], based on the
%             interquartile range of the samples. This method cannot be
%             used if N is given.
% spherData When the underlying density is truly normal, then the
%           approximation here is only valid if the cross terms of the
%           correlation are zero. To make that the case, if this parameter
%           is true, then the data is "sphered" --premultiplied by the
%           inverse of the square root (lower-triangular Cholesky
%           decomposition) of the sample covariance matrix, and then the
%           final result is multipled by the square root of the sample
%           covariance matrix to scale everything back. However, when this
%           function is used for approximations, sphering can sometimes
%           make thing worse. Thus, if this is false, then only the
%           diagonal terms of the sample covariance matrix are used and the
%           data is not squared. The default if this parameter is omitted
%           or an empty matrix is passed is true.
%         N If this parameter is provided, then param1 is assumed to be SR
%           and N is the number of samples that are being smoothed.
%           Otherwise, if this is omitted or an empty matrix is passed,
%           param1 is xi.
%
%OUTPUTS: H The kernel bandwidth matrix that can be used in the function
%           kernelApprox. This will be a lower-triangular matrix.
%         mu If xi is given, then this is the sample mean. Otherwise, this
%            is an empty matrix.
%         SR The root covariance matrix from the input or computed from xi.
%
%The methods in Chapter 3 of [1] are for a scalar distribution. We are able
%to apply them to a multivariate distribution by pre-multiplying the
%multivariate distribution by the inverse of the sample standard deviation
%of the points. If the samples were from a normal distribution (the
%approximation used here), then we can replace the scalar bandwidth
%formula with the solution for numDim-dimensions given in Chapter 2.4.2 of
%[2]. The pre-multiplication to decorrelate the dimensions is necessary for
%the use of that formula. Ultimately, we then have then rotate the final
%result back. The techniques from Chapter 3 of 1 that are used here only
%differ in how they try to obtain a robust estimate of the standard
%deviation values.
%
%REFERENCES:
%[1] B. W. Silverman, Density Estimation for Statistics and Data Analysis.
%    Chapman and Hall, 1986.
%[2] A. D. Bowman and A. Azzalini, Applied Smoothing Techniques for Data
%    Analysis: The Kernel Approach with S-Plus Illustrations. Oxford:
%    Clarendon Press, 2004.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4)
    N=[];
end

if(nargin<3||isempty(spherData))
    spherData=true;
end

if(nargin<2||isempty(method))
    method=1;
end
numDims=size(param1,1);

if(isempty(N))%xi is given
    xi=param1;
    [mu,S]=calcMixtureMoments(xi);
    SR=cholSemiDef(S,'lower',1);%Not fully triangular. but S=SR*SR'
    if(method~=1&&spherData)
        xi=SR\xi;
    end
    N=size(xi,2);%Number of points
else%S and N are given.
    if(method~=1)
        error('If N is given, then method=1 must be used.')
    end

    mu=[];
    SR=param1;
end

if(numDims>1&&spherData)
    sigma1=ones(numDims,1);
else
    sigma1=diag(SR);
end

switch(method)
    case 0%Use Equation 3.30 and 3.28 in [1].
        sigma2=interquartileRange(xi,2)/(GaussianD.invCDF(0.75)-GaussianD.invCDF(0.25));
        sigma=min([sigma1,sigma2],[],2);
    case 1%Use Equation 3.28 in [1].
        sigma=sigma1;
    case 2%Use the interquartile range (scaled appropriately) instead of
          %the standard deviation in [1]. This is Equation 3.29.
        sigma=interquartileRange(xi,2)/(GaussianD.invCDF(0.75)-GaussianD.invCDF(0.25));
    otherwise
        error('Unknown method specified')
end

h=(4/(N*(numDims+2)))^(1/(numDims+4))*sigma;

%Undo any rotation/scaling.
if(numDims>1&&spherData)
    %H is actually aking to the square root of a covariance matrix. h is a
    %list of scalars--the diagonls of the rotated matrix. This builds that
    %diagonal matrix and then scales it.
    H=SR*diag(h);
else
    H=diag(h);
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 inside = triangle_contains_point_1 ( t, p )

%*****************************************************************************80
%
%% TRIANGLE_CONTAINS_POINT_1 finds if a point is inside a triangle.
%
%  Discussion:
%
%    It is conventional to list the triangle vertices in counter clockwise
%    order.  However, this routine does not require a particular order
%    for the vertices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, logical INSIDE, is TRUE if the point is inside
%    the triangle or on its boundary.
%
  xsi = triangle_barycentric ( t, p );

  if ( any ( xsi(1:3,1) < 0.0 ) )
    inside = 0;
  else
    inside = 1;
  end

  return
end


function value = monomial_int01 ( dim_num, expon )

%*****************************************************************************80
%
%% MONOMIAL_INT01 returns the integral of a monomial over the [0,1] hypercube.
%
%  Discussion:
%
%    This routine evaluates a monomial of the form
%
%      product ( 1 <= dim <= dim_num ) x(dim)^expon(dim)
%
%    where the exponents are nonnegative integers.  Note that
%    if the combination 0^0 is encountered, it should be treated
%    as 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer EXPON(DIM_NUM), the exponents.
%
%    Output, real VALUE, the value of the integral of the
%    monomial over the [0,1] hypercube.
%
  value = 1.0 / prod ( expon(1:dim_num) + 1 );

  return
end


function [z,n] = phiflower(p)
%% PHIFLOWER level set function of a flower
%
% Author: Huayi Wei <weihuayi@xtu.edu.cn>

petal = 5;
xc = 0;
yc = 0;
r0 = 0.5;
r1 = 0.2;
theta = atan2((p(:,2) - yc),(p(:,1) - xc));
r = r0 + r1*sin(petal*theta);

z = (p(:,2)-yc).^2 + (p(:,1)-xc).^2 - r.^2;

if nargout == 2
    rp = petal*r1*cos(petal*theta);
    n1 = rp.*cos(theta) - r.*sin(theta);
    n2 = rp.*sin(theta) + r.*cos(theta);
    l = sqrt(n1.^2+n2.^2);
    n = [n2./l, -n1./l];
end
end


clear all; close all; clc

L=30; n=512; x2=linspace(-L/2,L/2,n+1); x=x2(1:n); % spatial discretization
k=(2*pi/L)*[0:n/2-1 -n/2:-1].';    % wavenumbers for FFT
V=x.^2.';             % potential
t=0:0.2:20;           % time domain collection points

u=exp(-0.2*(x-1).^2); % initial conditions
ut=fft(u);            % FFT initial data
[t,utsol]=ode45('harm_rhs',t,ut,[],k,V); % integrate PDE
for j=1:length(t)
  usol(j,:)=ifft(utsol(j,:));      % transforming back
end

u2=exp(-0.2*(x-0).^2);
ut=fft(u2);
[t,utsol]=ode45('harm_rhs',t,ut,[],k,V);
for j=1:length(t)
  usol2(j,:)=ifft(utsol(j,:));
end

figure(1)
subplot(2,2,2)
surfl(x,t,abs(usol)+2), shading interp, colormap(gray) %colormap([0 0 0])
hold on
pcolor(x,t,abs(usol)), shading interp, %colormap(gray)
view(20,25)
tv=0*x+20;
Vx=x.^2;
plot3(x(12:end-12),tv(12:end-12),Vx(12:end-12)/100+2,'k','Linewidth',[2])
set(gca,'Xlim',[-15 15],'Ylim',[0 20],'Zlim',[0 4],'Xtick',[-15 -10 -5 0 5 10 15],'Ytick',[0 10 20],'Ztick',[2 3 4], ...
    'Zticklabel',{'0','1','2'},'Fontsize',[15]);

figure(1)
subplot(2,2,1)
surfl(x,t,abs(usol2)+2), shading interp, colormap(gray) %colormap([0 0 0])
hold on
pcolor(x,t,abs(usol2)), shading interp, %colormap(gray)
view(20,25)
tv=0*x+20;
Vx=x.^2;
plot3(x(12:end-12),tv(12:end-12),Vx(12:end-12)/100+2,'k','Linewidth',[2])
set(gca,'Xlim',[-15 15],'Ylim',[0 20],'Zlim',[0 4],'Xtick',[-15 -10 -5 0 5 10 15],'Ytick',[0 10 20],'Ztick',[2 3 4], ...
    'Zticklabel',{'0','1','2'},'Fontsize',[15]);

%surfl(x,t,abs(usol)), colormap(gray)

for j=1:length(t)
    usol3(j,:)=usol(j,n:-1:1);
end

usym=[usol; usol3];

[U,S,V]=svd(usol.');
[U2,S2,V2]=svd(usol2.');
[U3,S3,V3]=svd(usym.');

figure(1)
subplot(4,2,6)
plot(100*diag(S)/sum(diag(S)),'ko','Linewidth',[2]), grid on
set(gca,'Xlim',[0 20],'Ylim',[0 80],'Ytick',[0 40 80],'Fontsize',[15])
xlabel('')

subplot(4,2,5)
plot(100*diag(S2)/sum(diag(S2)),'ko','Linewidth',[2]), grid on
set(gca,'Xlim',[0 20],'Ylim',[0 80],'Ytick',[0 40 80],'Fontsize',[15])
xlabel('')

figure(2)
subplot(3,1,3)
sn=[-1 1 1 1 -1];
for j=1:5
  nrm=sqrt(trapz(x,U(:,j).^2));
  Up(:,j)=real(U(:,j))*sn(j)/nrm;
end
plot(x,real(Up(:,1:5)),'Linewidth',[2])
set(gca,'Xlim',[-5 5],'Xtick',[-5 -2.5 0 2.5 5],'Ylim',[-1 1],'Ytick',[-1 -0.5 0 0.5 1],'Fontsize',[15])
legend('mode 1','mode 2','mode 3','mode 4','mode 5')

subplot(3,1,2)
sn=[-1 -1 1 -1 -1];
for j=1:5
  nrm=sqrt(trapz(x,U2(:,j).^2));
  Up2(:,j)=real(U2(:,j))*sn(j)/nrm;
end
plot(x,real(Up2(:,1:5)),'Linewidth',[2])
set(gca,'Xlim',[-5 5],'Xtick',[-5 -2.5 0 2.5 5],'Ylim',[-1 1],'Ytick',[-1 -0.5 0 0.5 1],'Fontsize',[15])
% subplot(4,1,4)
% sn=[-1 1 1 1 -1];
% for j=1:5
%   Up3(:,j)=real(U3(:,j))*sn(j)/norm(U3(:,j));
% end
% plot(x,real(Up3(:,1:5)),'Linewidth',[2])
% set(gca,'Xlim',[-5 5],'Xtick',[-5 -2.5 0 2.5 5],'Ylim',[-0.2 0.2],'Ytick',[-0.2 -0.1 0 0.1 0.2],'Fontsize',[15])

h=[1+0*x; 2*x; 4*x.^2-2; 8*x.^3-12*x; 16*x.^4-48*x.^2+12];
for j=0:4
   phi(:,j+1)=(1/(sqrt(factorial(j)*(2^j)*sqrt(pi))).*exp(-x.^2/2).*h(j+1,:)).';
   nrm=sqrt(trapz(x,phi(:,j+1).^2));
   phi2(:,j+1)=phi(:,j+1)/nrm;
end
subplot(3,1,1), plot(x,phi2,'Linewidth',[2]),
set(gca,'Xlim',[-5 5],'Xtick',[-5 -2.5 0 2.5 5],'Ylim',[-1 1],'Ytick',[-1 -0.5 0 0.5 1],'Fontsize',[15])


function [x,M]=semidefQuadProgEqConst(B,a,A,b)
%%SEMIDEFQUADPROGEQCONST Solve a quadratic programming problem involving a
%       semidefinite matrix (or a positive defintie matrix) where there are
%       only equality constraints, no inequality constraints. Specifically,
%       this algorithm solves the optimization problem:
%        minimize_x x'*B*x+2*a'*x
%        subject to A*x=b
%       If B is positive semidefinite, then if a solution exists, an
%       infinite number actually exist. This function returns one solution
%       and a matrix that can be used to find the other solutions.
%
%INPUTS: B An nXn real positive definite or positive semidefinite,
%          symmetric matrix.
%        a An nX1 real vector.
%        A A numConstXn real matrix. This can be an omitted or empty matrix
%          can be passed if there are no constraints. numConst<=n. There
%          shouldn't be any redundant constraints.
%        b A numConstX1 real vector.
%
%OUTPUTS: x The nX1 solution.
%         M An nXn matrix. All other solutions are of the form
%           xOther=x+M*y, where y is a an nX1 real vector.
%
%This function implements the equations of Chapter IV of [1]. Note that
%based on the constraints, it is possible for no solution to exist. This
%function is run assuming that a solution exists; it does not check for
%soluion non-existence.
%
%EXAMPLE:
%This is example 4.1 in [1]. x will be [9;18;35]/20.
% B=[1, 2,-1;
%    2, 4,-2;
%   -1,-2,1];
% a=[2;4;1];
% A=[1,2,1];
% b=4;
% [x,M]=semidefQuadProgEqConst(B,a,A,b)
%One sees that the third row and column of M is all zeros. Also,
%M(:,2)=-M(:,1)/2, so the matrix is singular. Thus, the second column of M
%can be used as a basis. So, to get any new  point, we can use x+M(:,2)*v
%(for a scalar v) or we could use x+5*M(:,2)*v, which is the same as the
%solution given in [1].
%
%REFERENCES:
%[1] D. L. Nelson, "Quadratic programming techniques using matrix
%    pseudoinverses," Ph.D. dissertation, Texas Tech University, Lubbock,
%    TX, Dec. 1969.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(A))
    %If there are no constraints, call the unconstrained algorithm.
    [x,M]=semidefQuadProgUnconst(B,a);
    return;
end

n=length(a);
I=eye(n,n);
pinvA=pinv(A);
diffA=(I-pinvA*A);
C=diffA*B*diffA;
pinvC=pinv(C);

%A combination of Equation 4.3 and 4.5.
x=pinvA*b+diffA*pinv(C)*diffA*(a-B*pinvA*b);
M=diffA*(I-pinvC*C);

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.


% Section 8.5.3: Analytic center of a set of linear inequalities
% Boyd & Vandenberghe "Convex Optimization"
% Joëlle Skaf - 04/29/08
%
% The analytic center of a set of linear inequalities and equalities:
%           a_i^Tx <= b_i   i=1,...,m,
%           Fx = g,
% is the solution of the unconstrained minimization problem
%           minimize    -sum_{i=1}^m log(b_i-a_i^Tx).

% Input data
randn('state', 0);
rand('state', 0);
n = 10;
m = 50;
p = 5;
tmp = randn(n,1);
A = randn(m,n);
b = A*tmp + 10*rand(m,1);
F = randn(p,n);
g = F*tmp;

% Analytic center
cvx_begin
    variable x(n)
    minimize -sum(log(b-A*x))
    F*x == g; %#ok
cvx_end

disp(['The analytic center of the set of linear inequalities and ' ...
      'equalities is: ']);
disp(x);


% By Sherif A. Tawfik, Faculty of Engineering, Cairo University
function y=findzero(fun,x0,x1,varargin)
% fun is the function that we need to compute its root.
% x0 and x1 are two arguments to the function such that the root that we
% seek lie between them and the function has different sign at these two
% points
% varargin are the other arguments of the function

% the value of the function at the first point
f0=feval(fun,x0,varargin{:});

%the value of the function at the second point
f1=feval(fun,x1,varargin{:});

% check that the sign of the function at x0 and x1 is different. Otherwise
% report an error
if sign(f0)==sign(f1)
    error('the function at the two endpoints must be of opposite signs');
end

%find a closer point to the root using the method of chords. In the method
%of chords we simply connect the two points (x0, f(x0))  and (x1, f(x1))
%using a straight line and compute the intersection point with this line
%and the horizontal axis. This new point is closer to the desired root
x=x0 - f0 * ((x1-x0)/(f1-f0));

%evaluate the function at this new point
f=feval(fun,x,varargin{:});

% enter this root as long as the difference between the two points that
% sandwitch the desired root is larger than a certain threshold
while abs(f)>2^-52
    % we keep one of the two old points that has a different sign than the
    % new point and we overwrite the other old point with the new point
    if sign(f)==sign(f0)
        x0=x;
        f0=f;
    else
        x1=x;
        f1=f;
    end
    x=x0 - f0 * ((x1-x0)/(f1-f0));
    f=feval(fun,x,varargin{:});
end
% at the end of the loop we reach the root with the desired precision and
% it is given by x
y=x;


function mat_gram = GetKernelMat(data1,data2,kernel_type,kernel_param)
% Ccomputes the Gram matrix of a specified kernel function.
%Input: data1, data2-Input data matix
%            kernel_type: 'linear' | 'poly' | 'rbf'|'intersect'|'sigmoid'
% 2016-10-16 by jlfeng

n1=size(data1,1);
n2=size(data2,1);

switch kernel_type
    case 'linear'
        mat_gram=data2*data1';
    case 'poly'
        mat_gram=(data2*data1'+1).^kernel_param;
    case 'polyhomog'
        mat_gram=(data2*data1').^kernel_param;
    case 'rbf'
        mat_gram = exp(-(repmat(sum(data1.*data1,2)',n2,1) + ...
                repmat(sum(data2.*data2,2),1,n1) - 2*data2*data1') ...
                /(2*kernel_param^2));
    case 'intersect'
        mat_gram=zeros(n2,n1);
        for kk=1:n1
            temp=min(data2,repmat(data1(kk,:),[n2 1]));
            mat_gram(:,kk)=sum(temp,2);
        end
    case 'sigmod'
        mat_gram=tanh(data2*data1'*kernel_param(1)+kernel_param(2));
    otherwise
        error('Unknown kernel function.');
end


function [c,E,exitVal]=minimaxPolyFit(f,xSpan,n,RelTol,maxIter)
%MINIMAXPOLYFIT Find an interpolating polynomial of order n to minimize
%               the maximum error approximating a scalar function f in the
%               range [xSpan(1);xSpan(2)]. The Remez algorithm is used.
%
%INPUTS: f A handle to the function that is to be approximated. The use
%          y=f(x) take a scalar x and returns a value y.
%    xSpan The range of inputs of x over which a minimax interpolating
%          polynomial is desired. The format is [xMin;xMax]. If this is
%          omitted or an empty matrix is passed, the values [-1;1] are
%          used.
%        n The degree of the polynomial approximation desired. If this is
%          omitted or an empty matrix is passed, the default value of n=5
%          is used. If too small a value is used and the function
%          oscillates a lot, the output polynomial will not be a true
%          minimax polynomial, because the algorithm assumes that there is
%          one maxima between regions chosen for selecting points. Also,
%          if f is an mth order polynomial, but is symmetric, then n
%          should be m+1 due to assumptions in the algorithm (the
%          resulting output will have the extra coefficient zero).
%   RelTol The relative tolerance of the maximum error used for
%          determining convergence. Iterations adjust the maximum error
%          observed with respect to points at which the function is
%          evaluated. When the maximum error stops changing across
%          iterations, convergence is assumed. The relative maximum error
%          is abs(E-EPrev)/abs(E); where E is the error and EPRev is the
%          error from the previous iteration. If this parameter is omitted
%          or an empty matrix is passed, a value of 1e-12 is used.
%  maxIter The maximum number of iterations for convergence. If omitted, a
%          value of 50 is used.
%
%OUTPUTS: c An interpolating polynomial such that polyval(c,xRel) gives the
%           interpolated value of f where xRel is the fractional distance
%           between xSpan(1) and xSpan(2). The value xRel ranges from 0 to
%           1. The development of the interpolation across 0 and 1 is often
%           numerically better than explicitly making the polynomial for
%           values from xSpan(1) to xSpan(2).
%         E The maximum error of the interpolating polynomial.
%   exitVal A value indicating how the algorithm terminated. Possible
%           values are:
%           0: The tolerance goal was met.
%           1: the maximum number of iterations was met.
%
%The algorithm used to fit a polynomial to an arbitrary nonlinear function
%is Remez algorithm. This is described ine Remez algorithm is well known
%and is described in Chapter 6 of [1].
%
%To initalize the Remez algorithm, one must choose a set of points in the
%region to be interpolated. One might be tempted to unifromely space the
%points, but as noted in [2] and Chaper 8.3 of [3], that can be a very bad
%idea. Rather, the use of Chebychev nodes is better and has much better
%asymptotic properties as the number of nodes increases.
%
%Given a set of points in xSpan, the algorithm find an interpolating
%polynomial that mades the magnitude of the error at all of the points the
%same and the sign of the error alternate. This is because there is a
%theorem that such an alternating equierror solution is a minimax solution.
%However, only a minimax solution for the initial set of points is given.
%Thus, the algorithm tries to move the points to maximize the error.
%
%Since the sign of the error between any two points flips, there is at
%least one zero between the points. This algorithm assumes that there is
%exactly one zero between the points, which should be reasonable if n is
%chosen to be large enough. In such an instance, the true error maxima of
%the function are between the zeros (and the outer bounds) of the function.
%Thus, the points can be moved there. Basically by maximizing the error at
%the chosen points, and getting an interpolating polynomial with equal
%error at the points, one finds the minimax approximation.
%
%As an example, consider interpolating the exponential function between 0
%and 2 with a sixth-order polynomial:
% f=@(x)exp(x);
% xSpan=[0;2];
% n=6;
% [c,E]=minimaxPolyFit(f,xSpan,n)
% %The maximum error E should be about 8.7e-6.
% %The resulting interpolating polynomial can be plotted as a fraction of
% %the distance between the points in xSpan. We will plot the error.
% numPoints=500;
% x=linspace(xSpan(1),xSpan(2),numPoints);
% xFrac=(x-xSpan(1))/(xSpan(2)-xSpan(1));
% error=polyval(c,xFrac)-exp(x);
% plot(x,error)
%
%REFERENCES:
%[1] V. K. Dzyadyk and I. A. Shevchuk, Theory of Uniform Approximation
%    of Functions by Polynomials, 1st ed. Berlin: Walter de Gruyter, 2008.
%[2] R. B. Platte, L. N. Trefethen, and A. B. Kuijlaars, "Impossibility of
%    fast stable approximation of analytic functions from equispaced
%    samples," SIAM Review, vol. 53, no. 2, pp. 308-318, 2011.
%[3] R. L. Burden and J. D. Faires, Numerical Analysis, 9th ed. Australia:
%    Brooks/ Cole, 2011.
%
%July 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(maxIter))
   maxIter=50;
end

if(nargin<4||isempty(RelTol))
   RelTol=1e-12;
end

if(nargin<3||isempty(n))
    n=5;
end

if(nargin<2||isempty(xSpan))
    xSpan=[-1;1];
end

%Step 1: Choose an initial set of nodes for interpolation.
%n+2 nodes are needed for an order-n polynomial approximation. The initial
%guess is set to the roots of the Chebyshev polynomial of the first kind.
%Such a choice is standard in most implementations of the Remez algorithm,
%because the Chebyshev polynomials distribute more nodes to the outside of
%the interval then to the center, reducing Runge's phenomenon.
k=(n+2):-1:1;
%The Chebyshev nodes for the (-1,1) span.
xNodes=cos((2*k(:)-1)/(2*(n+2))*pi);
%We are reducing the span of the nodes from -1/2 to 1/2 so that the
%interpolating polynomial on the output of the function takes an input from
%0 to 1.
xNodes=xNodes/2;

curIter=1;
EPrev=Inf;%This is used to determine convergence of the error estimate E.
while(1)
    %Step 2: Solve for the maximum error E and the coefficients of the
    %        interpolating polynomial P given the current set of nodes.
    %        This involves solving a linear system of equations.

    %Allocate space for the matrix.
    hMat=zeros(n+2,n+2);

    xDelta=(xNodes+1/2);
    hMat(:,1)=1;%The zeroth-order terms.
    for curCol=2:(n+1)
        hMat(:,curCol)=hMat(:,curCol-1).*xDelta;%This is xDelta.^(curCol-1)
    end

    %The last column corresponds to the E error term. This last column is
    %why the matrix is not a Vandermonde matrix.
    hMat(:,n+2)=(-1).^(0:(n+1));

    %Map the (-1,1) Chebyshev nodes into the range given by xSpan.
    xFNodes=(xSpan(2)-xSpan(1))*(xNodes+1/2)+xSpan(1);

    %Solve the system for the polynomial coefficients c and the error E
    coeffs=hMat\f(xFNodes);

    %The flipud is to make the ordering the same as that used by the
    %polyval function.
    c=flipud(coeffs(1:(n+1)));
    E=coeffs(n+2);

    relTolVal=abs(E-EPrev)/abs(E);

    %Check for convergence. Convergence is determined is the E value stops
    %changing significantly.
    %The check for the relative tolerance value being a nan deals with the
    %case where E=0 and E-EPrev=0.
    if(isnan(relTolVal)||relTolVal<RelTol)
        E=abs(E);
        exitVal=0;
        break;
    end

    %Return if the maximum number of iterations has passed.
    if(curIter==maxIter)
        E=abs(E);
        exitVal=1;
        break;
    end

    EPrev=E;

    %The coefficients c form the order-n minimax approximation of the
    %function at the n nodes. The error at those nodes has magnitude E.
    %However, these are most likely not the maximum error points for the
    %full function f. We want to move the points xNodes so that we are
    %minimizing the maximum error with respect to f in the interval.

    %Step 3: The Exchange Step. Given the current polynomial interpolation,
    %move the points to where it maximizes the magnitude of the error
    %function.

    %Step 3.1: Find the zeros of the error function between the control
    %          points.
    costFunZero=@(x)errorFun(x,xSpan,c,f);
    %There are n+1 points; the extra 2 points allocated are to make finding
    %the extrema in the next part of the step simpler.
    xIntervals=zeros(n+3,1);
    for curPoint=1:(n+1)
       %Find the zero of the error function between points curPoint and
       %curPoint+1.
       %Precision is arbitrary, but since X is from -1/2 to 1/2, it makes
       %sense to hard-code in the tolerance on X (and use a higher
       %tolernace than is the default)
       options=optimset('TolX',1e-14);
       xIntervals(curPoint+1)=fzero(costFunZero,[xNodes(curPoint),xNodes(curPoint+1)],options);
    end

    %Step 3.2: Find the extreme points and make them the new nodes.
    %The extrema are bracketed between each pair of roots. There are also
    %two more extrema located between the start of the interval xSpan(1)
    %and the first root and between the last root and the end of the
    %interval xSpan(2). We want to find all of the extrema. We will do line
    %searches to find those points. Those points are placed into xNodes as
    %they will form the new nodes for the next iteration.
    xIntervals(1)=-1/2;
    xIntervals(end)=1/2;

    costFunMax=@(x)negAbsErrorFun(x,xSpan,c,f);
    for curPoint=1:(n+2)
        %Find the maximum of the error function in the bounded interval
        %using a line search algorithm.
        options=optimset('TolX',1e-14);
        xNodes(curPoint)=fminbnd(costFunMax,xIntervals(curPoint),xIntervals(curPoint+1),options);
    end

    %Step 4: Iterate.
    curIter=curIter+1;
end
end

function val=errorFun(x,xSpan,c,f)
   %Map the (-1/2,1/2) Chebyshev node into the range given by xSpan.
   xf=(xSpan(2)-xSpan(1))*(x+1/2)+xSpan(1);

   val=polyval(c,x+1/2)-f(xf);
end

function val=negAbsErrorFun(x,xSpan,c,f)
   %Map the (-1/2,1/2) Chebyshev node into the range given by xSpan.
   xf=(xSpan(2)-xSpan(1))*(x+1/2)+xSpan(1);

   val=-abs(polyval(c,x+1/2)-f(xf));
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 q3 = quat_mul ( q1, q2 )

%*****************************************************************************80
%
%% QUAT_MUL multiplies two quaternions.
%
%  Discussion:
%
%    A quaternion is a quadruplet (A,B,C,D) of real numbers, which
%    may be written as
%
%      Q = A + Bi + Cj + Dk.
%
%    To multiply two quaternions, use the relationships:
%
%      i * j = -j * i = k
%      j * k = -k * j = i
%      k * i = -i * k = j
%      i * i =  j * j = k * k = -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Q1(4), Q2(4), the two quaternions to be multiplied.
%
%    Output, real Q3(4), the product of the two quaternions.
%
  q3(1) = q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);
  q3(2) = q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);
  q3(3) = q1(1) * q2(3) - q1(2) * q2(4) + q1(3) * q2(1) + q1(4) * q2(2);
  q3(4) = q1(1) * q2(4) + q1(2) * q2(3) - q1(3) * q2(2) + q1(4) * q2(1);

  return
end


% 改进的欧拉法求解初值问题
clear;
f = @(x,y) (y - 2 * x / y);
h = 0.1;   % 步长
x = 0:h:2;  % 离散点
y(1) = 1;   % 初值
y_xn = (1 + 2 * x).^(1/2);   % 解析解

% 改进的欧拉法
for i = 2 : length(x)
    yp = y(i-1) + h * f(x(i-1),y(i-1));
    yc = y(i-1) + h * f(x(i),yp);
    y(i) = (yp + yc) / 2;
end
y
y_xn
plot(x,y,'o-',x,y_xn,'k-');
legend('改进的欧拉法','精确解');


function angle = triangle_angles_3d ( t )

%*****************************************************************************80
%
%% TRIANGLE_ANGLES_3D computes the angles of a triangle in 3D.
%
%  Discussion:
%
%    The law of cosines is used:
%
%      C * C = A * A + B * B - 2 * A * B * COS ( GAMMA )
%
%    where GAMMA is the angle opposite side C.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(DIM_NUM,3), the triangle vertices.
%
%    Output, real ANGLE(3,1), the angles opposite
%    sides P1-P2, P2-P3 and P3-P1, in radians.
%

%
%  Compute the length of each side.
%
  a = sqrt ( sum ( ( t(1:3,1) - t(1:3,2) ).^2 ) );
  b = sqrt ( sum ( ( t(1:3,2) - t(1:3,3) ).^2 ) );
  c = sqrt ( sum ( ( t(1:3,3) - t(1:3,1) ).^2 ) );

  angle = zeros ( 3, 1 );
%
%  Take care of a special case.
%
  if ( a == 0.0 & b == 0.0 & c == 0.0 )
    angle(1:3,1) = 2.0 * pi / 3.0;
    return
  end

  if ( c == 0.0 | a == 0.0 )
    angle(1,1) = pi;
  else
    angle(1,1) = r8_acos ( ( c * c + a * a - b * b ) / ( 2.0 * c * a ) );
  end

  if ( a == 0.0 | b == 0.0 )
    angle(2,1) = pi;
  else
    angle(2,1) = r8_acos ( ( a * a + b * b - c * c ) / ( 2.0 * a * b ) );
  end

  if ( b == 0.0 | c == 0.0 )
    angle(3,1) = pi;
  else
    angle(3,1) = r8_acos ( ( b * b + c * c - a * a ) / ( 2.0 * b * c ) );
  end

  return
end


function imgOut = ConvertIPTtoICh(img, inverse)
%
%       imgOut = ConvertIPTtoICh(img, inverse)
%
%
%        Input:
%           -img: image to convert from IPT to ICh or from ICh to IPT.
%           -inverse: takes as values 0 or 1. If it is set to 1 the
%                     transformation from IPT to ICh is applied, otherwise
%                     the transformation from IChto IPT.
%
%        Output:
%           -imgOut: converted image in IPT or ICh (see inverse).
%
%     Copyright (C) 2016  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

check3Color(img);

imgOut = img;

if(inverse == 0)%ICh color space
    imgOut(:,:,2) = sqrt(img(:,:,2).^2 + img(:,:,3).^2);
    imgOut(:,:,3) = atan2(img(:,:,2), img(:,:,3));
else
    imgOut(:,:,2) = sin(img(:,:,3)) .* img(:,:,2);
    imgOut(:,:,3) = cos(img(:,:,3)) .* img(:,:,2);
end

end


function cdf = birthday_cdf ( n )

%*****************************************************************************80
%
%% BIRTHDAY_CDF returns the Birthday Concurrence CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 August 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of people whose birthdays have been
%    disclosed.
%
%    Output, real CDF, the probability that at least
%    two of the N people have matching birthays.
%
  if ( n < 1 )
    cdf = 0.0;
    return
  elseif ( 365 < n )
    cdf = 1.0;
    return
  end
%
%  Compute the probability that N people have distinct birthdays.
%
  cdf = 1.0;
  for i = 1 : n
    cdf = cdf * ( 365 + 1 - i ) / 365.0;
  end
%
%  Compute the probability that it is NOT the case that N people
%  have distinct birthdays.  This is the cumulative probability
%  that person 2 matches person 1, or person 3 matches 1 or 2,
%  etc.
%
  cdf = 1.0 - cdf;

  return
end


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];
z2 = a1 *Theta1';
a2 = [ones(size(z2),1) sigmoid(z2)];
z3 = a2*Theta2';
a3 = sigmoid(z3);

[predict_max, index_max] = max(a3, [], 2);

p = index_max;

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

end


function theta = polygonNormalAngle(points, ind)
%POLYGONNORMALANGLE Compute the normal angle at a vertex of the polygon
%
%   THETA = polygonNormalAngle(POLYGON, IND);
%   where POLYGON is a set of points, and IND is index of a point in
%   polygon. The function compute the angle of the normal cone localized at
%   this vertex.
%   If IND is a vector of indices, normal angle is computed for each vertex
%   specified by IND.
%
%   Example
%   % creates a simple polygon
%   poly = [0 0;0 1;-1 1;0 -1;1 0];
%   % compute normal angle at each vertex
%   theta = polygonNormalAngle(poly, 1:size(poly, 1));
%   % sum of all normal angle of a non-intersecting polygon equals 2xpi
%   % (can be -2xpi if polygon is oriented clockwise)
%   sum(theta)
%
%   See also:
%   polygons2d, polygonOuterNormal, normalizeAngle
%

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

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

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

theta = zeros(nv, 1);

for i = 1:nv
    % current vertex
    curr = points(ind(i), :);

    % previous and next vertices
    prev = points(mod(ind(i)-2, np)+1, :);
    next = points(mod(ind(i), np)+1, :);

    theta(i) = angle3Points(prev, curr, next) - pi;
end


function y = logn_lpdf(x,mu,sigma)
%LOGNPDF Lognormal log-probability density function (lpdf).
%
%   Y = LOGN_LPDF(X,MU,SIGMA) Returns the log of the lognormal pdf at
%   the values in X. The mean and standard deviation of log(Y) are
%   MU and SIGMA.
%
%   The size of Y is the common size of the input arguments. A scalar input
%   functions as a constant matrix of the same size as the other inputs.
%
%   Default values for MU and SIGMA are 0 and 1 respectively.

% Copyright (c) 2000 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

y = -0.5 * ((log(x) - mu)./sigma).^2 - log(x .* sqrt(2*pi) .* sigma);


% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function jacobian_symbolic_w
L3 = 0.38;

syms q1 q2

A01 = dh_sym(q1, L3, 0, pi/2);
A12 = dh_sym(q2, 0, 0, -pi/2);
A02 = A01*A12;

z0 = [0 0 1]';
z1 = A01(1:3,3);
z2 = A02(1:3,3);

Jw = [z0 z1 z2];

singularities = det(Jw)
singularities = simplify(singularities)
q=solve(singularities == 0)

function A = dh_sym(theta, d, a, alpha)
syms q1 q2
ca = cos(alpha);
sa = sin(alpha);
if abs(ca) < 1e-6
    ca = 0;
end
if abs(sa) < 1e-6
    sa = 0;
end

A=[cos(theta)  -ca*sin(theta)   sa*sin(theta)   a*cos(theta);
   sin(theta)   ca*cos(theta)  -sa*cos(theta)   a*sin(theta);
            0              sa             ca             d;
            0         0                     0              1];


function K = prtUtilQuadExpCovariance(x1, x2, theta0, theta1, theta2, theta3)
% xxx Need Help xxx
% K = prtUtilQuadExpCovariance(x1, x2, theta0, theta1, theta2, theta3)
%
% Common covariance function for Gaussian Processes
%
% Bishop, Pattern Recognition and Machine Learning Eq. 6.63

if nargin==3 && isvector(theta0) && numel(theta0)==4
    theta3 = theta0(4);
    theta2 = theta0(3);
    theta1 = theta0(2);
    theta0 = theta0(1);
else
    if nargin < 6 || isempty(theta3)
        theta3 = 0;
    end

    if nargin < 5 || isempty(theta2)
        theta2 = 0;
    end

    if nargin < 4 || isempty(theta1)
        theta1 = 1;
    end

    if nargin < 3 || isempty(theta0)
        theta0 = 1;
    end
end

K = theta0 * exp(-theta1/2 * prtDistanceEuclidean(x1,x2).^2) + theta2 + theta3*x1*x2';


function [ valLoss ] = HuberLoss( vX, paramDelta )
% ----------------------------------------------------------------------------------------------- %
% [ valLoss ] = HuberLoss( vX, paramDelta )
%   Applies the Huber Loss to the input vector.
% Input:
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - ballRadius    -   Ball Radius.
%                       Sets the Radius of the L1 Ball. For Unit L1 Ball
%                       set to 1.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
% Output:
%   - vX            -   Output Vector.
%                       The projection of the Input Vector onto the L1
%                       Ball.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Huber Loss (Wikipedia) - https://en.wikipedia.org/wiki/Huber_loss.
% Remarks:
%   1.  a
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     21/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

numElements = length(vX);

valLoss = 0;

for ii = 1:numElements
    if(abs(vX(ii)) <= paramDelta)
        valLoss = valLoss + (0.5 * vX(ii) * vX(ii));
    else
        valLoss = valLoss + (paramDelta * (abs(vX(ii)) - (0.5 * paramDelta)));
    end

end

end


function v=mai(x,lag)
%Syntax: v=mai(x,lag)
%____________________
%
% Calculates the mutual average information of a time series x for
% some time lag.
%
% v is the the value of the mutual average information.
% x is the time series.
% lag is the time lag.
%
% Alexandros Leontitsis
% Institute of Mathematics and Statistics
% University of Kent at Canterbury
% Canterbury
% Kent, CT2 7NF
% U.K.
% University e-mail: al10@ukc.ac.uk
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% May 25, 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % n is the time series length
   n=length(x);
end

if nargin<2 | isempty(lag)==1
   lag=0:min(n/2-1,20);
else
   % lag must be a vector
   if min(size(lag))>1
      error('The time lag must be a scalar or a vector.');
   end
   % lag must contain integers
   lag=round(lag);
   % lag values must be between 0 and n/2-1
   lag=lag(find(lag>=0 & lag<n/2));
   % lag must not be empty
   if isempty(lag)==1
      error('You must give another set of values for lag.')
   end
end

% The mutual average information
x=x-min(x);
x=x/max(x);
for i=1:length(lag)

   % Define the number of bins
   k=floor(1+log2(n-lag(i))+0.5);

   % If the time series has no variance then the MAI is 0
   if var(x,1)==0
      v(i)=0;
   else
      v(i)=0;
      for k1=1:k
         for k2=1:k
            ppp=find((k1-1)/k<x(1:n-lag(i)) & x(1:n-lag(i))<=k1/k ...
               & (k2-1)/k<x(1+lag(i):n) & x(1+lag(i):n)<=k2/k);
            ppp=length(ppp);
            px1=find((k1-1)/k<x(1:n-lag(i)) & x(1:n-lag(i))<=k1/k);
            px2=find((k2-1)/k<x(1+lag(i):n) & x(1+lag(i):n)<=k2/k);
            if ppp>0
               ppp=ppp/(n-lag(i));
               px1=length(px1)/(n-lag(i));
               px2=length(px2)/(n-lag(i));
               v(i)=v(i)+ppp*log2(ppp/px1/px2);
            end
         end
      end
   end
end


function pass = test_guide( pref )
% Test Chebfun2 guide commands. This is not exclusive by checks the main
% commands.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e3*pref.cheb2Prefs.chebfun2eps;

% commands from guide1:
f = chebfun2(@(x,y) cos(x.*y));

pass(1) = abs( sum2(f) - 3.784332281468732 ) < tol;

x = chebfun2(@(x,y) x, [-2 3 -4 4]);
y = chebfun2(@(x,y) y, [-2 3 -4 4]);

op = @(x,y) 1./( 2 + cos(.25 + x.^2.*y + y.^2) );
g = ( 2 + cos(.25 + x.^2.*y + y.^2) );
f = 1./g;

x = linspace(-2,3);
y = linspace(-4,4);
[xx,yy] = meshgrid(x,y);
pass(2) = ( norm( f(xx,yy) - op(xx,yy), inf) ) < 200*tol;

f = chebfun2(@(z) sin(z));
pass(3) = ( abs( f(1+1i) -  sin(1+1i) ) ) < tol;

f = chebfun2(@(z) sin(z)-sinh(z),2*pi*[-1 1 -1 1]);
x = linspace(-2*pi,2*pi);
[xx,yy] = meshgrid(x);
pass(4) = ( norm( f(xx+1i*yy) - f(xx,yy), inf)) < tol;
pass(5) = ( norm( f(xx+1i*yy) - sin(xx+1i*yy)+sinh(xx+1i*yy), inf)) < 1e3*tol;

% commands from guide2:
f = chebfun2(@(x,y) sin(10*x.*y), [0 pi/4 0 3]);
pass(6) = ( norm( sum(f) - chebfun(@(x) sin(15*x).^2./x/5,[0 pi/4])' )) < tol;
pass(7) = ( norm( sum(f,2) - chebfun(@(y) sin(5*pi*y/4).^2./y/5,[0 3]) )) < tol;
pass(8) = ( abs( sum2(f) - sum(sum(f)) )) < tol;

F = @(x,y) exp(-(x.^2 + y.^2 + cos(4*x.*y)));
I1 = quad2d(F,-1,1,-1,1,'AbsTol',tol);
I2 = sum(sum(chebfun2(F)));
I3 = sum2(chebfun2(F));
pass(9) = ( abs(I1 - I2)) < tol;
pass(10) = ( abs(I2 - I3)) < tol;
pass(11) = ( abs(I1 - I3)) < tol;

f = chebfun2( 'exp(-(x.^2 + y.^2 +4*x.*y))' );
pass(12) = ( abs( norm(f) - sqrt(sum2(f.^2)) ) < tol );

f = chebfun2( @(x,y) exp(-1./( sin(x.*y) + x ).^2) );
g = chebfun2( @(x,y) cos(exp(-1./( sin(x.*y) + x ).^2)) );
h = chebfun2( @(x,y) (exp(-1./( sin(x.*y) + x ).^2)).^5 );
pass(13) = ( norm( cos(f) - g ) ) < tol;
pass(14) = ( norm( f.^5 - h )) < tol;

runge = chebfun2( @(x,y) 1./( .01 + x.^2 + y.^2 )) ;
pass(15) = ( abs(mean2(runge) - 3.796119578934828)) < tol;
pass(16) = ( abs(mean(mean(runge)) - 3.796119578934828)) < tol;

f = chebfun2(@(x,y) exp(-(x.^2 + 3*x.*y+y.^2) ));
pass(17) = ( norm( cumsum(cumsum(f),2) - cumsum2(f) )) < tol;

f = chebfun2(@(x,y) cos(10*x.*y.^2) + exp(-x.^2)); % chebfun2 object
C = chebfun(@(t) t.*exp(10i*t),[0 1]);             % spiral curve

pass(18) = ( abs(sum(f(C)) - 1.613596461872283)) < tol;

% commands from guide4:
f = chebfun2(@(x,y) sin(x+1i*y));   % a holomorphic function
u = real(f); v = imag(f);           % real and imaginary parts

pass(19) = ( norm(diff(u) - (-diff(v,1,2)))) < tol;
pass(20) = ( norm(diff(u,1,2) - diff(v)) ) < tol;        % Do the Cauchy-Riemann eqns hold?

d = [0 1 0 2];
F = chebfun2v(@(x,y) sin(x.*y), @(x,y) cos(y),d);  % calling the constructor
f = chebfun2(@(x,y) sin(x.*y),d); g = chebfun2(@(x,y) cos(y),d);
G = [f;g];
plaw = abs((2*norm(F)^2 + 2*norm(G)^2) - (norm(F+G)^2 + norm(F-G)^2));
pass(21) = ( plaw < tol );

f = chebfun2(@(x,y) cos(10*x.*y.^2) + exp(-x.^2));  % chebfun2
F = gradient(f);                                    % gradient (chebfun2v)
C = chebfun(@(t) t.*exp(10i*t),[0 1]);              % spiral curve
v = integral(F,C);ends = f(cos(10),sin(10))-f(0,0); % line integral
pass(22) = ( abs(v-ends) ) < tol;

% commands from guide6:
r1 = 1; r2 = 1/3;   % inner and outer radius
d = [0 2*pi 0 2*pi];
u = chebfun2(@(u,v) u,d);
v = chebfun2(@(u,v) v,d);
F = [-(r1+r2*cos(v)).*sin(u); (r1+r2*cos(v)).*cos(u); r2*sin(v)];  % torus
G = F./3;  % full 3D divergence of G is 1 because F = [x;y;z].
pass(23) = ( abs( integral2(dot(G,normal(F))) - 2*pi^2*r1*r2.^2 ) < tol );


function mu = trapezmf(z,a,b,c,d)
%TRAPEZMF Trapezoidal membership function.
%   MU = TRAPEZMF(Z,A,B,C,D) computes a fuzzy membership function with a
%   trapezoidal shape. Z is the input variable and can be a vector of
%   any length. A, B, C, and D are scalar parameters that define the
%   trapezoidal shape. The parameters must be ordered so that A <= B, B
%   <= C, and C <= D.
%
%       MU = 0                          Z < A
%       MU = (Z - A) ./ (B - A)         A <= Z < B
%       MU = 1                          B <= Z < C
%       MU = 1 - (Z - C) ./ (D - C)     C <= Z < D
%       MU = 0                          D <= Z
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

mu = zeros(size(z));

up_ramp_region = (a <= z) & (z < b);
top_region = (b <= z) & (z < c);
down_ramp_region = (c <= z) & (z < d);

mu(up_ramp_region) = 1 - (b - z(up_ramp_region))./(b - a);
mu(top_region) = 1;
mu(down_ramp_region) = 1 - (z(down_ramp_region) - c)./(d - c);


function [rho pval] = circ_corrcl(alpha, x)
%
% [rho pval ts] = circ_corrcc(alpha, x)
%   Correlation coefficient between one circular and one linear random
%   variable.
%
%   Input:
%     alpha   sample of angles in radians
%     x       sample of linear random variable
%
%   Output:
%     rho     correlation coefficient
%     pval    p-value
%
% References:
%     Biostatistical Analysis, J. H. Zar, p. 651
%
% PHB 6/7/2008
%
% Circular Statistics Toolbox for Matlab

% By Philipp Berens, 2009
% berens@tuebingen.mpg.de - www.kyb.mpg.de/~berens/circStat.html

if size(alpha,2) > size(alpha,1)
	alpha = alpha';
end

if size(x,2) > size(x,1)
	x = x';
end

if length(alpha)~=length(x)
  error('Input dimensions do not match.')
end

n = length(alpha);

% compute correlation coefficent for sin and cos independently
rxs = corr(x,sin(alpha));
rxc = corr(x,cos(alpha));
rcs = corr(sin(alpha),cos(alpha));

% compute angular-linear correlation (equ. 27.47)
rho = sqrt((rxc^2 + rxs^2 - 2*rxc*rxs*rcs)/(1-rcs^2));

% compute pvalue
pval = 1 - chi2cdf(n*rho^2,2);


function u=EOBurg(u,t,dx,dir,bndcond)
%%
% Solves Burgers' equation u_t + 0.5*u^2_x = 0 by the Engquist-Osher method.
%

%%
% Initial setup
if nargin<5,
  bndcond='Neumann';
end;
if nargin<4,
  dir=1;
end;
if (dir>2),
  error(strcat('You can easily extend this to',num2str(dir)','D yourself ...'));
end;
% Some things ensuring that you solve along the columns of u. Must
% transpose if u is a row vector or if dir = 2.
transpose=0;
S=size(u);
N=S(1);
if ((N==1)||(dir==2)),
  u=u';
  transpose=1;
end;
maxspeed=max(max(abs(u)));
CFL=0.95;
dt=CFL*dx/maxspeed;
nstep=ceil(t/dt);
dt=t/nstep;
lambda=dt/dx;
%%
% Solving by EO metod
for j=1:nstep,
  uext=set_extend('iden',u,bndcond);
  u=u-lambda*diff(eoflux(uext));
end;
if (transpose)
  u=u';
end;

%% eoflux
function f=eoflux(u)
  S=size(u);
  N=S(1);
  MM=max(u(1:N-1,:),0.0); mm=min(u(2:N,:),0.0);
  f=0.5*(MM.^2+mm.^2);


close all; clear all; clc;

wave = spx.wavelet.daubechies.wavelet_function(4, 8, 1024, 4);
subplot(221);
t = (1:1024)./1024;
plot(t(300:800),wave(300:800));
ylim([-.25 +.25]);
title(' D4 Wavelet ');

wave = spx.wavelet.daubechies.scaling_function(4, 8, 1024, 4);
subplot(222);
t = (1:1024)./1024;
plot(t(300:800),wave(300:800));
ylim([-.25 +.25]);
title(' D4 Scaling function ');


function [ i, j ] = i4_to_pascal ( k )

%*****************************************************************************80
%
%% I4_TO_PASCAL converts a linear index to Pascal triangle coordinates.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/triangle_integrals/i4_to_pascal.m
%
%  Discussion:
%
%    We describe the grid points in Pascal's triangle in two ways:
%
%    As a linear index K:
%
%                     1
%                   2   3
%                 4   5   6
%               7   8   9   10
%
%    As elements (I,J) of Pascal's triangle:
%
%                     0,0
%                  1,0   0,1
%               2,0   1,1    0,2
%            3,0   2,1   1,2    0,3
%
%  Example:
%
%     K  I  J
%
%     1  0  0
%     2  1  0
%     3  0  1
%     4  2  0
%     5  1  1
%     6  0  2
%     7  3  0
%     8  2  1
%     9  1  2
%    10  0  3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer K, the linear index of the (I,J) element.
%    1 <= K.
%
%    Output, integer I, J, the Pascal indices.
%
  if ( k <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_TO_PASCAL - Fatal error!\n' );
    fprintf ( 1, '  K must be positive.\n' );
    error ( 'I4_TO_PASCAL - Fatal error!' );
  end

  d = i4_to_pascal_degree ( k );

  j = k - ( d * ( d + 1 ) ) / 2 - 1;
  i = d - j;

  return
end


function value = not1d ( x1, x2, x3 )

%*****************************************************************************80
%
%% NOT1D differentiates a factor for serendipity basis functions.
%
%  Discussion:
%
%    not1(x1,x2,x3) evaluates at the point x1, the basis factor that
%    is 0 at x2 and 1 at x3:
%
%      not1(x1,x2,x3) = ( x1 - x2 ) / ( x3 - x2 )
%
%    This function returns the derivative of the factor with respect to x1:
%
%      not1d(x1,x2,x3) = 1 / ( x3 - x2 );
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X1, the evaluation point.
%
%    Input, real X2, X3, values that define the factor.
%
%    Output, real VALUE, the derivative of the basis function factor.
%
  value = 1.0 / ( x3 - x2 );

  return
end


function V_times = crossTimesMatrix(V)
% CROSSTIMESMATRIX
%   V_TIMES = CROSSTIMESMATRIX(V) returns a 3x3 (or a series of 3x3) cross times matrices of input vector(s) V
%
%   Input:
%       V a 3xN matrix, rpresenting a series of 3x1 vectors
%
%   Output:
%       V_TIMES (Vx) a series of 3x3 matrices where V_times(:,:,i) is the Vx matrix for the vector V(:,i)
%
% 	Babak Taati, 2003
%   (revised 2009)

[a,b] = size(V);
V_times = zeros(a, 3, b);

% V_times(1,1,:) = 0;
V_times(1,2,:) = - V(3,:);
V_times(1,3,:) = V(2,:);

V_times(2,1,:) = V(3,:);
% V_times(2,2,:) = 0;
V_times(2,3,:) = - V(1,:);

V_times(3,1,:) = - V(2,:);
V_times(3,2,:) = V(1,:);
% V_times(3,3,:) = 0;


%normu Normalize image points to be used for LS estimation.
%   A = normu(u) finds normalization matrix A so that mean(A*u)=0 and
%   mean(||A*u||)=sqrt(2). (see Hartley: In Defence of 8-point Algorithm,
%   ICCV`95).
%
%   Parameters:
%      u ... Size (2,N) or (3,N). Points to normalize.
%      A ... Size (3,3). Normalization matrix.

function A = normu(u)

if size(u,1)==3, u = p2e(u); end

m        = mean(u')'; % <=> mean (u,2)
u        = u - m*ones(1,size(u,2));
distu    = sqrt(sum(u.*u));
r        = mean(distu)/sqrt(2);
if ~r, A = []; return; end     % one of degenarate configurations
A        = diag([1/r 1/r 1]);
A(1:2,3) = -m/r;


% 预测世界石油产量
%% 历史数据
clear;clc;
x = (1994 : 2003)';    % 年份
year = 1990;
x = x - year;
y = [67.052, 68.008, 69.803, 72.024, 73.400, 72.063, 74.669, 74.487, 74.065, 76.777]';   % 百万桶每日
plot(x,y,'rp');
%% 直线拟合
C = [x.^0, x.^1];
A = C' * C;  % 法方程系数矩阵
b = C' * y;  % 法方程向量
p_1 = A \ b   % 一次多项式系数，升幂排列
RMSE_1 = sqrt(sum((y - C * p_1).^2)) / sqrt(length(C))
%% 抛物线拟合
C = [x.^0, x.^1, x.^2];
A = C' * C;  % 法方程系数矩阵
b = C' * y;  % 法方程向量
p_2 = A \ b   % 二次多项式系数，升幂排列
RMSE_2 = sqrt(sum((y - C * p_2).^2)) / sqrt(length(C))
%% 三次曲线拟合
C = [x.^0, x.^1, x.^2, x.^3];
A = C' * C;  % 法方程系数矩阵
b = C' * y;  % 法方程向量
p_3 = A \ b   % 三次多项式系数，升幂排列
RMSE_3 = sqrt(sum((y - C * p_3).^2)) / sqrt(length(C))
%% 开始预测
x1=x(end) + (1:5)';   % 待预测时间, 10年
xi=[x;x1];
p = p_1;   % 指定模型
plot(x + year,y,'*',xi + year,polyval(flip(p),xi),x1 + year,polyval(flip(p),x1),'rp');
% 结论：预测结果仅供参考


function [ pn, dist ] = circle_arc_point_near_2d ( r, center, theta1, ...
  theta2, p )

%*****************************************************************************80
%
%% CIRCLE_ARC_POINT_NEAR_2D : nearest point on a circular arc.
%
%  Discussion:
%
%    A circular arc is defined by the portion of a circle (R,C)
%    between two angles (THETA1,THETA2).
%
%    Thus, a point (X,Y) on a circular arc satisfies
%
%      ( X - C(1) ) * ( X - C(1) ) + ( Y - C(2) ) * ( Y - C(2) ) = R * R
%
%    and
%
%      Theta1 <= Theta <= Theta2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2), the center of the circle.
%
%    Input, real THETA1, THETA2, the angles defining the arc,
%    in radians.  Normally, THETA1 < THETA2.
%
%    Input, real P(2), the point to be checked.
%
%    Output, real PN(2), a point on the circular arc which is
%    nearest to the point.
%
%    Output, real DIST, the distance to the nearest point.
%
  ndim = 2;
%
%  Special case, the zero circle.
%
  if ( r == 0.0 )
    pn(1:ndim) = center(1:ndim);
    dist = sqrt ( sum ( ( p(1:ndim) - pn(1:ndim) ).^2 ) );
    return
  end
%
%  Determine the angle made by the point.
%
  theta = atan4 ( p(2) - center(2), p(1) - center(1) );
%
%  If the angle is between THETA1 and THETA2, then you can
%  simply project the point onto the arc.
%
  if ( r8_modp ( theta  - theta1,  2.0 * pi ) <= ...
       r8_modp ( theta2 - theta1,  2.0 * pi ) )

    r2 = sqrt ( sum ( ( p(1:ndim) - center(1:ndim) ).^2 ) );

    pn(1:ndim) = center(1:ndim) + ( p(1:ndim) - center(1:ndim) ) * r / r2;
%
%  Otherwise, if the angle is less than the negative of the
%  average of THETA1 and THETA2, it's on the side of the arc
%  where the endpoint associated with THETA2 is closest.
%
%  As usual, MATLAB has some ninny problem with using cosine in a vector.
%  So I have to write the assignment as two scalar assignments.
%
  elseif ( r8_modp ( theta - 0.5 * ( theta1 + theta2 ), 2.0 * pi ) <= pi )

    pn(1) = center(1) + r * cos ( theta2 );
    pn(2) = center(2) + r * sin ( theta2 );
%
%  Otherwise, the endpoint associated with THETA1 is closest.
%
  else

    pn(1) = center(1) + r * cos ( theta1 );
    pn(2) = center(2) + r * sin ( theta1 );

  end

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

  return
end


% Spectral factorization using Kolmogorov 1939 approach.
% (code follows pp. 232-233, Signal Analysis, by A. Papoulis)
%
% Computes the minimum-phase impulse response which satisfies
% given auto-correlation.
%
% Input:
%   r: top-half of the auto-correlation coefficients
%      starts from 0th element to end of the auto-corelation
%      should be passed in as a column vector
% Output
%   h: impulse response that gives the desired auto-correlation

function h = spectral_fact(r)

% length of the impulse response sequence
n = length(r);

% over-sampling factor
mult_factor = 100;        % should have mult_factor*(n) >> n
m = mult_factor*n;

% computation method:
% H(exp(jTw)) = alpha(w) + j*phi(w)
% where alpha(w) = 1/2*ln(R(w)) and phi(w) = Hilbert_trans(alpha(w))

% compute 1/2*ln(R(w))
w = 2*pi*((0:m-1)/m);
R = [ ones(m,1) 2*cos(kron(w',1:n-1)) ]*r;
alpha = 1/2*log(R);

% find the Hilbert transform
alphatmp = fft(alpha);
alphatmp(floor(m/2)+1:m) = -alphatmp(floor(m/2)+1:m);
alphatmp(1) = 0;
alphatmp(floor(m/2)+1) = 0;
phi = real(ifft(1j*alphatmp));

% now retrieve the original sampling
index  = find(rem(0:m-1,mult_factor)==0);
alpha1 = alpha(index);
phi1   = phi(index);

% compute the impulse response (inverse Fourier transform)
h = real(ifft(exp(alpha1+1j*phi1),n));


function dist=KLDiv(P,Q)
%  dist = KLDiv(P,Q) Kullback-Leibler divergence of two discrete probability
%  distributions
%  P and Q  are automatically normalised to have the sum of one on rows
% have the length of one at each
% P =  n x nbins
% Q =  1 x nbins or n x nbins(one to one)
% dist = n x 1

if size(P,2)~=size(Q,2)
    error('the number of columns in P and Q should be the same');
end

if sum(~isfinite(P(:))) + sum(~isfinite(Q(:)))
   error('the inputs contain non-finite values!')
end

% normalizing the P and Q
if size(Q,1)==1
    Q = Q ./sum(Q);
    P = P ./repmat(sum(P,2),[1 size(P,2)]);
    temp =  P.*log(P./repmat(Q,[size(P,1) 1]));
    temp(isnan(temp))=0;% resolving the case when P(i)==0
    dist = sum(temp,2);

elseif size(Q,1)==size(P,1)

    Q = Q ./repmat(sum(Q,2),[1 size(Q,2)]);
    P = P ./repmat(sum(P,2),[1 size(P,2)]);
    temp =  P.*log(P./Q);
    temp(isnan(temp))=0; % resolving the case when P(i)==0
    dist = sum(temp,2);
end


% Test file for periodic eigenvalue problems.
function pass = test_eigs_periodic(pref)

% Get the preferences.
if ( nargin < 1 )
    pref = cheboppref;
end

tol = 1e2*pref.bvpTol;

%% EXAMPLE USING TRIGCOLLOC.
% Solving
%   -u'' = lambda*u,
% for x in [0 2*pi],  subject to
% periodic boundary conditions.

% Define the domain we're working on.
dom = [0, 2*pi];

% Assign the equation to a chebop N such that N(u) = lambda*u.
L = chebop(@(u) -diff(u, 2), dom);

% Assign boundary conditions to the chebop.
L.bc = 'periodic';

% Number of eigenvalue and eigenmodes to compute.
k = 7;

% Solve the eigenvalue problem with FOURIER.
[V, D] = eigs(L, k);
D = diag(D);

% Exact solution.
Dexact = [0 1 1 4 4 9 9]';
pass(1) = norm(D - Dexact, inf) < tol;
pass(2) = isequal(get(V{1}.funs{1}, 'tech'), @trigtech);

%% ANOTHER EXAMPLE USING TRIGCOLLOC.
% Solving
%   -u'' + 2*q*cos(2*x)*u = lambda*u,
% for x in [0 2*pi],  subject to
% periodic boundary conditions.

% Define the domain we're working on.
dom = [0, 2*pi];

% Assign the equation to a chebop N such that N(u) = lambda*u.
q = 2;
L = chebop(@(x, u) -diff(u, 2) + 2*q*cos(2*x).*u, dom);

% Assign boundary conditions to the chebop.
L.bc = 'periodic';

% Number of eigenvalue and eigenmodes to compute.
k = 7;

% Solve the eigenvalue problem with FOURIER.
[V, D] = eigs(L, k);
D = diag(D);

% Solutions from WolframAlpha.
Dwolfram = [ -1.513956885056520;
             -1.390676501225323;
              2.379199880488686;
              3.672232706497191;
              5.172665133358294;
              9.140627737766440;
              9.370322483621104 ];
pass(3) = norm(D - Dwolfram, inf) < tol;
pass(4) = isequal(get(V{1}.funs{1}, 'tech'), @trigtech);

end


function rgb = hsi2rgb(hsi)
%HSI2RGB Converts an HSI image to RGB.
%   RGB = HSI2RGB(HSI) converts an HSI image to RGB, where HSI is
%   assumed to be of class double with:
%     HSI(:,:,1) = hue image, assumed to be in the range [0,1] by
%                    having been divided by 2*pi.
%     HSI(:,:,2) = saturation image, in the range [0,1].
%     HSI(:,:,3) = intensity image, in the range [0,1].
%
%   The components of the output image are:
%     RGB(:,:,1) = red.
%     RGB(:,:,2) = green.
%     RGB(:,:,3) = blue.
%
%   All the conversion equations are explained in Chapter 7 of DIPUM3E.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Extract the individual HSI component images.
H = hsi(:,:,1) * 2 * pi;
S = hsi(:,:,2);
I = hsi(:,:,3);

% Implement the conversion equations.
R = zeros(size(hsi,1),size(hsi,2));
G = zeros(size(hsi,1),size(hsi,2));
B = zeros(size(hsi,1),size(hsi,2));

% RG sector (0 <= H < 2*pi/3).
idx = find( (0 <= H) & (H < 2*pi/3));
B(idx) = I(idx).*(1 - S(idx));
R(idx) = I(idx).*(1 + S(idx).*cos(H(idx))./...
                                          cos(pi/3 - H(idx)));
G(idx) = 3*I(idx) - (R(idx) + B(idx));

% BG sector (2*pi/3 <= H < 4*pi/3).
idx = find( (2*pi/3 <= H) & (H < 4*pi/3) );
R(idx) = I(idx).*(1 - S(idx));
G(idx) = I(idx).*(1 + S(idx).*cos(H(idx) - 2*pi/3)./...
                    cos(pi - H(idx)));
B(idx) = 3*I(idx) - (R(idx) + G(idx));

% BR sector.
idx = find( (4*pi/3 <= H) & (H <= 2*pi));
G(idx) = I(idx).*(1 - S(idx));
B(idx) = I(idx).*(1 + S(idx).*cos(H(idx) - 4*pi/3)./...
                                           cos(5*pi/3 - H(idx)));
R(idx) = 3*I(idx) - (G(idx) + B(idx));

% Combine all three results into an RGB image. Clip to [0,1] to
% compensate for floating-point arithmetic rounding effects.
rgb = cat(3,R,G,B);
rgb = max(min(rgb,1),0);