function value = r8_beta_pdf ( alpha, beta, rval )

%*****************************************************************************80
%
%% R8_BETA_PDF evaluates the PDF of a beta distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2013
%
%  Author:
%
%    Original FORTRAN90 version by Guannan Zhang.
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, real ALPHA, BETA, shape parameters.
%    0.0 < ALPHA, BETA.
%
%    Input, real RVAL, the point where the PDF is evaluated.
%
%    Output, real VALUE, the value of the PDF at RVAL.
%
  if ( alpha <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BETA_PDF - Fatal error!\n' );
    fprintf ( 1, '  Parameter ALPHA is not positive.\n' );
    error ( 'R8_BETA_PDF - Fatal error!' );
  end

  if ( beta <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BETA_PDF - Fatal error!\n' );
    fprintf ( 1, '  Parameter BETA is not positive.\n' );
    error ( 'R8_BETA_PDF - Fatal error!' );
  end

  if ( rval < 0.0 || 1.0 < rval )

    value = 0.0;

  else

    temp = r8_gamma_log ( alpha + beta ) - r8_gamma_log ( alpha ) ...
      - r8_gamma_log ( beta );

    value = exp ( temp ) * rval ^ ( alpha - 1.0 ) ...
      * ( 1.0 - rval ) ^ ( beta - 1.0 );

  end

  return
end


function a = jacobi ( m, n )

%*****************************************************************************80
%
%% JACOBI returns the JACOBI matrix.
%
%  Formula:
%
%    if ( J = I - 1 )
%      A(I,J) = 0.5 * sqrt ( ( 4 * J^2 ) / ( 4 * J^2 - 1 ) )
%    else if ( J = I + 1 )
%      A(I,J) = 0.5 * sqrt ( ( 4 * (J-1)^2 ) / ( 4 * (J-1)^2 - 1 ) )
%    else
%      A(I,J) = 0
%
%  Example:
%
%    M = 4, N = 4
%
%    0            0.577350269  0            0
%    0.577350269  0            0.516397779  0
%    0            0.516397779  0            0.507092553
%    0            0            0.507092553  0
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A has a zero diagonal.
%
%    The eigenvalues of A are the zeros of the Legendre polynomial
%    of degree N.  They lie symmetrically in [-1,1], and are also
%    the nodes of Gauss-Legendre quadrature.  For the case of N = 4,
%    these eigenvalues are
%
%      [ -0.861136312, -0.339981044, +0.339981044, +0.861136312 ].
%
%    It follows that A is singular when N is odd.
%
%    The J-th Gauss-Legendre weight is twice the square of the first
%    component of the J-th eigenvector of A.  For the case of N = 4,
%    the eigenvector matrix is:
%
%      -0.417046     -0.571028     -0.571028    0.417046
%       0.622037      0.336258     -0.336258    0.622038
%      -0.571028      0.417046      0.417046    0.571028
%       0.336258     -0.622037      0.622038    0.336258
%
%    and the corresponding weights are
%
%      [ 0.347854845, 0.652145155, 0.652145155, 0.347854845 ]
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Lloyd Trefethen, David Bau,
%    Numerical Linear Algebra,
%    SIAM, 1997, pages 287-292.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n

      if ( j == i - 1 )
        a(i,j) = 0.5 * sqrt ( ( 4 * j * j ) / ( 4 * j * j - 1 ) );
      elseif ( j == i + 1 )
        a(i,j) = 0.5 * sqrt ( ( 4 * ( j - 1 ) * ( j - 1 ) ) ...
          / ( 4 * ( j - 1 ) * ( j - 1 ) - 1 ) );
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function [I J] = itriu(sz, k)
% function [I J] = itriu(sz) % OR
% I = itriu(sz) OR
%
% Return the subindices [I J] (or linear indices I if single output call)
% in the purpose of extracting an upper triangular part of the matrix of
% the size SZ. Input k is optional shifting. For k=0, extract from the main
% diagonal. For k>0 -> above the diagonal, k<0 -> below the diagonal
%
% This returnd same as [...] = find(triu(ones(sz),k))
% - Output is a column and sorted with respect to linear indice
% - No intermediate matrix is generated, that could be useful for large
%   size problem
% - Mathematically, A(itriu(size(A)) is called (upper) "half-vectorization"
%   of A
%
% Example:
%
% A = [ 7     5     4
%       4     2     3
%       9     1     9
%       3     5     7 ]
%
% I = itriu(size(A))  % gives [1 5 6 9 10 11]'
% A(I)                % gives [7 5 2 4  3  9]' OR A(triu(A)>0)
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% Date: 21/March/2009

if isscalar(sz)
    sz = [sz sz];
end
m=sz(1);
n=sz(2);

% Main diagonal by default
if nargin<2
    k=0;
end

nc = n-max(k,0); % number of columns of the triangular part
lo = ones(nc,1); % lower row indice for each column
hi = min((1:nc).'-min(k,0),m); % upper row indice for each column

if isempty(lo)
    I = zeros(0,1);
    J = zeros(0,1);
else
    c=cumsum([0; hi-lo]+1); % cumsum of the length
    I = accumarray(c(1:end-1), (lo-[0; hi(1:end-1)]-1), ...
                   [c(end)-1 1]);
    I = cumsum(I+1); % row indice
    J = accumarray(c,1);
    J(1) = 1 + max(k,0); % The row indices starts from this value
    J = cumsum(J(1:end-1)); % column indice
end

if nargout<2
    % convert to linear indices
    I = sub2ind([m n], I, J);
end

end % itriu


function w = compute(w);

%COMPUTE calculates the fast fourier transform of a waveform object
% WAVEFORM = COMPUTE(WAVEFORM) calculate frequency spectrum of a waveform.
% The results of the fast fourier transform is added as new fields
% in the waveform:
%   FFT_FREQ is a vector of frequencies with N samples
%   FFT_AMP is a vector of spetral amplitudes
%   FFT_PHASE is a vector of phases
%   FFT_DOM is the scalar frequency of the maximum amplitude
%           peak (or dominant frequency. This could change)

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

% CHECK ARGUMENTS
if ~strcmpi(class(w),'waveform')
    error('First input must be a waveform object');
end

% STEP THROUGH WAVFORMS ADDING NEW FIELDS
[N,M] = size(w);
for i = 1:N*M
    Fn = get(w(i),'NYQ');
    x = get(w(i),'DATA');
    NFFT=2.^(ceil(log(length(x))/log(2)));  % Next highest power of 2
    FFTX=fft(x,NFFT);                       % Take fft, padding with zeros.
    NumUniquePts = ceil((NFFT+1)/2);
    FFTX=FFTX(1:NumUniquePts);              % throw out neg frequencies
    MX=abs(FFTX);                           % Take magnitude of X
    MX=MX*2;                                % Multiply by 2
    MX=MX/length(x);
    PX=phase(FFTX);                           % Take magnitude of X
    f=(0:NumUniquePts-1)*2/NFFT;
    f=f*Fn;
    w(i) = addfield(w(i),'FFT_FREQ',f');
    w(i) = addfield(w(i),'FFT_AMP',MX);
    w(i) = addfield(w(i),'FFT_PHASE',PX);
    a = find(MX == max(MX));
    w(i) = addfield(w(i),'FFT_DOM',f(a));
end;


function R2 = RodriguesConversion(R1)
% Rodrigues - converts a Rodrigues rotation vector to rotation matrix or vice versa.
%
% Usage:
%           R2 = Rodrigues(R1)
%
% Input:
%           R1 : Rodrigues rotation vector (3x1 or 1x3) or rotation matrix (3x3).
%
% Output:
%           R2 : rotation matrix (3x3) or Rodrigues rotation vector (3x1 or 1x3).
%
% This code follows the algorithm given by
% [1] R. Hartley and A. Zisserman "Multiple View Geometry in Computer Vision,"
%     Cambridge, pp.583-585, 2003.
%
% Kim, Daesik
% Intelligent Systems Research Center
% Sungkyunkwan Univ. (SKKU), South Korea
% E-mail  : daesik80@skku.edu
% Homepage: http://www.daesik80.com
%
% July 2008  - Original version.

[r,c] = size(R1);

%% Rodrigues Rotation Vector to Rotation Matrix
if ((r == 3) && (c == 1)) || ((r == 1) && (c == 3))
    wx = [  0   -R1(3)  R1(2);
        R1(3)   0   -R1(1);
        -R1(2)  R1(1)   0   ];

    R1_norm = sqrt(R1(1)^2 + R1(2)^2 + R1(3)^2);

    if (R1_norm < eps)
        R2 = eye(3);
    else
        R2 = eye(3) + sin(R1_norm)/R1_norm*wx + (1-cos(R1_norm))/R1_norm^2*wx^2;
    end

    %% Rotation Matrix to Rodrigues Rotation Vector
elseif (r == 3) && (c == 3)
    w_norm = acos((trace(R1)-1)/2);
    if (w_norm < eps)
        R2 = [0 0 0]';
    else
        R2 = 1/(2*sin(w_norm))*[R1(3,2)-R1(2,3);R1(1,3)-R1(3,1);R1(2,1)-R1(1,2)]*w_norm;
    end
end


function [CartPoint,latLong]=randEllipsoidLoc(N,a,f)
%%RANDELLIPSOIDLOC Generate a random point uniformly distributed on the
%              surface of a reference ellipsoid, such as the WGS-84
%              reference ellipsoid. The point is provided in Cartesian
%              coordinates as well as in terms of latitude and longitude.
%
%INPUTS: N The number of random samples on the reference ellipsoid to draw.
%        a The semi-major axis of the reference ellipsoid. If this argument
%          is omitted, the value in Constants.WGS84SemiMajorAxis is used.
%        f The flattening factor of the reference ellipsoid. If this
%          argument is omitted, the value in Constants.WGS84Flattening is
%          used.
%
%OUTPUTS: CartPoint A 3XN matrix of random Cartesian [x;y;z] points on the
%                   ellipsoid with the given semi-major axis and flattening
%                   factor.
%           latLong A 2XN matrix of ellipsoidal latitude and longitude
%                   corresponding to points in CartPoint.
%
%The problem of generating a uniformly distributed sample on a reference
%ellipsoid is number 28 in Chapter 3.4.1 of [1], where the algorithm to do
%so is given in the back of the book. That algorithm is implemented here,
%where the general ellipsoid formulation has been modified to support the
%typical parameterization in terms of a semimajor axis and a flattening
%factor. The algorithm is a rejection sampling method.
%
%Large flattening factor values can make the algorithm slow.
%
%REFERENCES:
%[1] D. Knuth, The Art of Computer Programming: Seminumerical Algorithms,
%    3rd ed. Reading, MA: Addison-Wesley, 1998, vol. 2.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

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

%The equation for a reference ellipsoid is
%(x^2+y^2)/a^2+z^2/(a^2*(1-f)^2)=1.
%The algorithm describe in Knuth's book wants things paramterized in terms
%of c(1)*x^2+c(2)*y^2+c(3)*z^2=1. Thus, the coefficients are
c(1,1)=1/a^2;
c(2,1)=1/a^2;
c(3,1)=1/(a^2*(1-f)^2);

%The c's have to be sorted. This means that the ordering of the components
%in x, y, and z will be wrong, so indices to undo the sorting when saving
%the final result need to be found.
[c,sortIdx]=sort(c,'descend');
unsortIdx=1:3;
unsortIdx=unsortIdx(sortIdx);

n=3;%The number of dimensions.

%Allocate space for the return variables.
CartPoint=zeros(3,N);

for curSamp=1:N
    while(1)
        %First, generate a random point on the unit sphere
        y=randDirVec(3);
        rho=sqrt(sum(c.*y.^2));

        %Find K
        if(n*c(n)>=c(1))
            K=sqrt(c(n)^(n-1));
        else
            K=sqrt(((n+1)/(c(1)+c(n)))^(n+1)*(c(1)*c(n)/n)^n);
        end

        %Generate a uniform random variable U
        U=rand(1);

        if(rho^(n+1)*U<K*sqrt(sum(c.^2.*y.^2)))
            CartPoint(unsortIdx,curSamp)=y/rho;
            break;
        end
    end
end

if(nargout>1)
    point=Cart2Ellipse(CartPoint,[],a,f);
    latLong=point(1:2,:);
end
end

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


function a = toeplitz_pds ( m, n, x, y )

%*****************************************************************************80
%
%% TOEPLITZ_PDS returns the TOEPLITZ_PDS matrix.
%
%  Discussion:
%
%    TOEPLITZ is a Toeplitz matrix that is positive definite symmetric.
%
%  Formula:
%
%    A(I,J) = sum ( 1 <= K <= M ) Y(K) * cos ( 2 * PI * X(K) * (I-J) )
%
%  Example:
%
%    N = 5, M = 5,
%    X = ( -0.0625, - 0.03125, 0.0, 0.03125, 0.0625 ),
%    Y = ( 0.2, 0.2, 0.2, 0.2, 0.2)
%
%    1.000000  0.961866  0.852395  0.685661  0.482843
%    0.961866  1.000000  0.961866  0.852395  0.685661
%    0.852395  0.961866  1.000000  0.961866  0.852395
%    0.685661  0.852395  0.961866  1.000000  0.961866
%    0.482843  0.685661  0.852395  0.961866  1.000000
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A is positive definite or positive semi-definite, depending on
%    the values of X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Cybenko, Charles Van Loan,
%    Computing the minimum eigenvalue of a symmetric positive definite
%    Toeplitz matrix,
%    SIAM Journal on Scientific and Statistical Computing,
%    Volume 7, 1986, pages 123-131.
%
%  Parameters:
%
%    Input, integer M, the number of terms of W and X.
%
%    Input, integer N, the order of A.
%
%    Input, real X(M), used to define the matrix.
%
%    Input, real Y(M), a set of positive weights used to define the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n
      a(i,j) = 0.0;
      for k = 1 : m
        angle = 2.0 * pi * x(k) * ( i - j );
        a(i,j) = a(i,j) + y(k) * cos ( angle );
      end
    end
  end

  return
end


function cg_test ( )

%*****************************************************************************80
%
%% CG_TEST tests CG.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CG_TEST:\n' );
  fprintf ( 1, '  CG uses the Conjugate Gradient \n' );
  fprintf ( 1, '  iterative method to approximate the solution \n' );
  fprintf ( 1, '  of a linear system A * x = b.\n' );

  n = 10;

  A = zeros ( n, n );

  for i = 1 : n
    A(i,i) = 2.0;
  end
  for i = 1 : n-1
    A(i,i+1) = -1;
  end
  for i = 2 : n
    A(i-1,i) = -1;
  end
  x = [ 1 : n ]';
  b = A * x;
  x = ones ( n, 1 );

  M = 0;
  max_it = 10;
  tol = 0.0001;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this example, the order of the system is N = %d\n', n );
  fprintf ( 1, '  The matrix A is the simple tridiagonal -1, 2, -1.\n' );
  fprintf ( 1, '  The correct solution is x = [ 1, 2, ..., n].\n' );
  fprintf ( 1, '  The right hand side b is determined by computing A * x.\n' );
  fprintf ( 1, '  The exact x is then replaced by a vector of all 1''s for\n' );
  fprintf ( 1, '  use as a starting guess.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Other parameters are set as follows:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The maximum number of steps is %d.\n', max_it );
  fprintf ( 1, '  The error tolerance is %f\n', tol );

  [ x, error_norm, iter, flag ] = cg ( A, x, b, M, max_it, tol );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The CG routine has returned with FLAG = %d\n', flag );
  if ( flag == 0 )
    fprintf ( 1, '  This indicates that the iteration has converged.\n' );
  elseif ( flag == 1 )
    fprintf ( 1, '  This indicates that the iteration has NOT converged.\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of iterations taken was %d\n', iter );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The L2 norm of the error per iteration:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : iter
    fprintf ( 1, '  %4d  %f\n', i, error_norm(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The computed solution vector X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %f\n', i, x(i) );
  end

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

  return
end


function a = conex3_inverse ( n )

%*****************************************************************************80
%
%% CONEX3_INVERSE returns the inverse of the CONEX3 matrix.
%
%  Example:
%
%    N = 5
%
%     1  0  0  0  0
%     1  1  0  0  0
%     2  1  1  0  0
%     4  2  1  1  0
%    -8 -4 -2 -1 -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Cline, RK Rew,
%    A set of counterexamples to three condition number estimators,
%    SIAM Journal on Scientific and Statistical Computing,
%    Volume 4, 1983, pages 602-611.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( i < n )

        if ( j < i )
          a(i,j) = 2.0^(i-j-1);
        elseif ( i == j )
          a(i,j) = 1.0;
        else
          a(i,j) = 0.0;
        end

      elseif ( i == n )

        if ( j < i )
          a(i,j) = - 2.0^(i-j-1);
        else
          a(i,j) = -1.0;
        end

      end

    end
  end

  return
end


function [A,U,V] = regutm(m,n,s)
%REGUTM Test matrix for regularization methods.
%
% [A,U,V] = regutm(m,n,s)
%
% Generates a random m-times-n matrix A such that A*A' and A'*A
% are oscillating.  Hence, in the SVD of A,
%    A = U*diag(s)*V',
% the number of sign changes in U(:,i) and V(:,i) is exactly i-1.
%
% The third argument s specifies the singular values of A.  If not
% present, then s = logspace(0,round(log10(eps)),n).

% Reference: P. C. Hansen, "Test matrices for regularization methods",
% SIAM J. Sci. Comput. 16 (1995), 506--512.

% Per Christian Hansen, IMM, 07/30/97.

% Initialization.
if (nargin==1), n = m; end
if (nargin<3), s = logspace(0,round(log10(eps)),min(m,n)); end

% Special treatment of the case m < n.
if (m < n), [A,V,U] = regutm(n,m,s); A = A'; return, end

% Generate random bidiagonal matrix with nonnegative elements.
if (n < 100), mu = .222*n + .0278*n^2; else mu = 3*n; end
B = abs(diag(randn(n,1)+mu) + diag(randn(n-1,1)+mu,1));

% Compute the SVD of B.
[U,dummy,V] = svd(B); clear dummy

% Repeat if m > n.
if (m > n)
  clear U
  B = abs(diag(randn(m,1)+mu) + diag(randn(m-1,1)+mu,1));
  [U,dummy] = svd(B); clear dummy, U = U(:,1:n);
end

% Compute A.
A = U*diag(s)*V';


function a = hadamard ( m, n )

%*****************************************************************************80
%
%% HADAMARD returns a HADAMARD matrix.
%
%  Definition:
%
%    A Hadamard matrix is a square matrix A of order N, whose entries are
%    only +1's or -1's, with the property that:
%
%      A * A' = N * I.
%
%  Notes:
%
%    A Hadamard matrix must be of order 1, 2, or else a multiple of 4.
%    It is not known whether a Hadamard matrix exists for every multiple
%    of 4.
%
%    The method used here allows the user to request a Hadamard matrix
%    of any rectangular order, M by N.  The algorithm then essentially
%    finds the largest powers of 2 that are less than or equal to M and
%    N, and produces a Hadamard-like matrix in that space, setting the
%    rest of the matrix to 0.  Thus, the matrix returned by this routine
%    is only a Hadamard matrix if M = N = a power of 2.
%
%  Formula:
%
%    The following recursive formula is used to produce a series of
%    Hadamard matrices of increasing size.
%
%    H(0) = [1]
%
%    H(1) = [ H(0)  H(0) ] = [ 1  1]
%           [ H(0) -H(0) ]   [ 1 -1]
%
%    H(2) = [ H(1)  H(1) ] = [ 1  1  1  1]
%           [ H(1) -H(1) ]   [ 1 -1  1 -1]
%                            [ 1  1 -1 -1]
%                            [ 1 -1 -1  1]
%
%    and so on.
%
%  Properties:
%
%    All entries of a Hadamard matrix are either +1 or -1.  Matrices
%    produced by this routine will be +1 or -1 up to a certain row
%    and column, beyond which the entries will be zero.
%
%    The Hadamard matrices produced by this routine have the property
%    that the first row and column are entirely 1's, although this
%    is not a requirement for a Hadamard matrix.
%
%    The matrices produced by this algorithm are (loosely) symmetric,
%    although that is not required for a Hadamard matrix.
%
%    Hadamard matrices exhibit the maximum possible relative growth of pivot
%    elements during Gaussian elimination with complete pivoting.
%
%    The inverse of a Hadamard matrix of order N is A itself,
%    scaled by 1.0/N.  Thus 1.0/sqrt(N) times a Hadamard matrix
%    yields a symmetric matrix which is its own inverse, or
%    "involutional".
%
%    A is integral: int ( A ) = A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory and David Karney,
%    Example 3.14,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, New York, 1969, page 42,
%    LC: QA263.G68.
%
%    William Pratt,
%    Digital Image Processing,
%    John Wiley and Sons, 1978.
%
%    Herbert Ryser,
%    Combinatorial Mathematics,
%    John Wiley and Sons, 1963.
%
%  Parameters:
%
%    Input, integer M, the row order of A.
%
%    Input, integer N, the column order of A.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HADAMARD - Fatal error!\n' );
    fprintf ( 1, '  Input value of M = %d\n', m );
    fprintf ( 1, '  but M must be positive.\n' );
    error ( 'HADAMARD - Fatal error!' );
  end

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HADAMARD - Fatal error!\n' );
    fprintf ( 1, '  Input value of N = %d\n', n );
    fprintf ( 1, '  but N must be positive.\n' );
    error ( 'HADAMARD - Fatal error!' );
  end

  a(1,1) = 1.0;

  nn = 1;

  while ( nn < n | nn < m )

    for i = 1 : nn
      for j = 1 : nn

        if ( i <= m & j+nn <= n )
          if ( 2 * nn <= n )
            a(i,j+nn) = a(i,j);
          else
            a(i,j+nn) = 0.0;
          end
        end

        if ( i + nn <= m & j <= n )
          if ( 2 * nn <= m )
            a(i+nn,j) = a(i,j);
          else
            a(i+nn,j) = 0.0;
          end
        end

        if ( i + nn <= m & j + nn <= n )
          if ( 2 * nn <= m & 2 * nn <= n )
            a(i+nn,j+nn) = - a(i,j);
          else
            a(i+nn,j+nn) = 0.0;
          end
        end

      end
    end

    nn = 2 * nn;

  end

  return
end


function qwgw_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests QWGW for the Gegenbauer weight.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2014
%
%  Author:
%
%    John Burkardt
%

%
%  Set the quadrature interval and number of points.
%
  a = -1.0;
  b = +1.0;
  n = 5;
%
%  Set the weight function parameter.
%
  alpha = 0.25;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03:\n' );
  fprintf ( 1, '  Compute points and weights for Gauss quadrature\n' );
  fprintf ( 1, '  with the Gegenbauer weight w(x) = (1-x^2)^alpha.\n' );
  fprintf ( 1, '  Order N = %d\n', n );
  fprintf ( 1, '  ALPHA = %g\n', alpha );
  fprintf ( 1, '  Interval = [%g,%g]\n', a, b );
%
%  Set the recursion coefficients.
%
  aj = zeros ( n, 1 );
  bj = zeros ( n, 1 );

  aj(1:n) = 0.0;

  for j = 1 : n - 1
    jr = j;
    bj(j) = ( jr * ( 2.0 * alpha + jr ) ) ...
      / ( 4.0 * ( alpha + jr )^2 - 1.0 );
  end
  bj(n) = 0.0;

  bj(1:n) = sqrt ( bj(1:n) );

  mu0 = gamma ( alpha + 1.0 ) * gamma ( 0.5 ) / gamma ( alpha + 1.5 );
%
%  Compute the points and weights.
%
  [ x, w ] = sgqf ( n, aj, bj, mu0 );

  r8vec_print ( n, x, '  Abscissas:' );
  r8vec_print ( n, w, '  Weights:' );

  return
end


function [a3,a2,a1,a0] = createTraj3(theta0,thetaf,thetad0,thetadf,tstart,tfinal)
	% inputs : initial position, velocity + final position, velocity + initial and final times
	% output : a vector specifying the polynomial and can be used with poly functions such as : polyder, polyval, etc.
	% create a 3rd order trajectory
	% example:
	% createTraj3(10,30,0,0,0,1)
	%
	%
	% By: Reza Ahmadzadeh - Matlab/Octave - 2013
	T = tfinal - tstart;
	a0 = theta0;
	a1 = thetad0;
	a2 = (-3 * (theta0 - thetaf) - (2 * thetad0+thetadf )*T)/ T ^ 2;
	a3 = (2 * (theta0 - thetaf) + (thetad0+thetadf )*T)/ T ^ 3;

end


function [ d2, p2 ] = poly_power ( d1, p1, n )

%*****************************************************************************80
%
%% POLY_POWER computes a power of a polynomial.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/triangle_integrals/poly_power.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer D1, the degree of the polynomial.
%
%    Input, real P1(M1), the polynomial coefficients.
%    M1 = ((D1+1)*(D1+2))/2.
%
%    Input, integer N, the nonnegative integer power.
%
%    Output, integer D2, the degree of the power polynomial.
%    D2 = N * D1.
%
%    Output, real P2(M2), the polynomial power.
%    M2 = ((D2+1)*(D2+2))/2.
%

%
%  Create P2, a polynomial representation of 1.
%
  d2 = 0;
  m2 = ( ( d2 + 1 ) * ( d2 + 2 ) ) / 2;
  p2 = zeros ( m2, 1 );
  p2(1) = 1.0;
%
%  Iterate N times:
%    P2 <= P2 * P1
%
  for i = 1 : n
    [ d2, p2 ] = poly_product ( d2, p2, d1, p1 );
  end

  return
end


function geometry_test001 ( )

%*****************************************************************************80
%
%% TEST001 tests ANGLE_CONTAINS_POINT_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 6;
  n_angle = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST001\n' );
  fprintf ( 1, '  ANGLE_CONTAINS_POINT_2D sees if a point\n' );
  fprintf ( 1, '  lies within an angle.\n' );
  fprintf ( 1, '\n' );
%
%  An acute angle (45 degrees)
%
  for j = 1 : ntest

    if ( j == 1 )

      p1(1:2,1) = [ 1.0; 0.0 ];
      p2(1:2,1) = [ 0.0; 0.0 ];
      p3(1:2,1) = [ 1.0; 1.0 ];

    elseif ( j == 2 )

      p1(1:2,1) = [ 1.0; 0.0 ];
      p2(1:2,1) = [ 0.0; 0.0 ];
      p3(1:2,1) = [ 0.0; 1.0 ];

    elseif ( j == 3 )

      p1(1:2,1) = [ 1.0; -1.0 ];
      p2(1:2,1) = [ 0.0;  0.0 ];
      p3(1:2,1) = [ 0.0;  1.0 ];

    elseif ( j == 4 )

      p1(1:2,1) = [  1.0; 0.0 ];
      p2(1:2,1) = [  0.0; 0.0 ];
      p3(1:2,1) = [ -1.0; 0.0 ];

    elseif ( j == 5 )

      p1(1:2,1) = [ 1.0;  0.0 ];
      p2(1:2,1) = [ 0.0;  0.0 ];
      p3(1:2,1) = [ 0.0; -1.0 ];

    elseif ( j == 6 )

      p1(1:2,1) = [ 1.0;  0.0 ];
      p2(1:2,1) = [ 0.0;  0.0 ];
      p3(1:2,1) = [ 1.0; -0.01 ];

    end

    r8vec_print ( 2, p1, '  Vertex P1' )
    r8vec_print ( 2, p2, '  Vertex P2' )
    r8vec_print ( 2, p3, '  Vertex P3' )

    fprintf ( 1, '\n' );
    fprintf ( 1, '       X            Y       Inside?\n' );
    fprintf ( 1, '\n' );

    for i = 0 : n_angle

      thetar = i * 2.0 * pi / n_angle;
%
%  For some bizarre MATLAB peculiarity, I can't say
%    p(1:2) = [ cos ( thetar ), sin ( thetar ) ];
%  MATLAB (Student 6.5) complains that COS has the wrong number
%  of arguments.  But write it out like this, and magically, everything
%  is fine.  People think I'm crabby for no reason...
%
      p(1,1) = cos ( thetar );
      p(2,1) = sin ( thetar );

      inside = angle_contains_point_2d ( p1, p2, p3, p );

      fprintf ( 1, '  %12f  %12f  %1d\n', p(1:2,1), inside );

    end

  end

  return
end


clear all, close all, clc

t = (-3:.01:3)';

Utrue = [cos(17*t).*exp(-t.^2) sin(11*t)];
Strue = [2 0; 0 .5];
Vtrue = [sin(5*t).*exp(-t.^2) cos(13*t)];

X = Utrue*Strue*Vtrue';
figure, imshow(X);

%%
sigma = 1;
Xnoisy = X+sigma*randn(size(X));
figure, imshow(Xnoisy);

%%
[U,S,V] = svd(Xnoisy);

N = size(Xnoisy,1);
cutoff = (4/sqrt(3))*sqrt(N)*sigma; % Hard threshold
r = max(find(diag(S)>cutoff)); % Keep modes w/ sig > cutoff
Xclean = U(:,1:r)*S(1:r,1:r)*V(:,1:r)';
figure, imshow(Xclean)

%%
cdS = cumsum(diag(S))./sum(diag(S));  % Cumulative energy
r90 = min(find(cdS>0.90));  % Find r to capture 90% energy

X90 = U(:,1:r90)*S(1:r90,1:r90)*V(:,1:r90)';
figure, imshow(X90)

%% plot singular values

semilogy(diag(S),'-ok','LineWidth',1.5), hold on, grid on
semilogy(diag(S(1:r,1:r)),'or','LineWidth',1.5)
plot([-20 N+20],[cutoff cutoff],'r--','LineWidth',2)
axis([-10 610 .003 300])
rectangle('Position',[-5,20,100,200],'LineWidth',2,'LineStyle','--')

figure
semilogy(diag(S),'-ok','LineWidth',1.5)
hold on, grid on
semilogy(diag(S(1:r,1:r)),'or','LineWidth',1.5)
plot([-20 N+20],[cutoff cutoff],'r--','LineWidth',2)
axis([-5 100 20 200])

figure
plot(cdS,'-ok','LineWidth',1.5)
hold on, grid on
plot(cdS(1:r90),'ob','LineWidth',1.5)
plot(cdS(1:r),'or','LineWidth',1.5)
set(gca,'XTick',[0 300 r90 600],'YTick',[0 .5 0.9 1.0])
xlim([-10 610])
plot([r90 r90 -10],[0 0.9 0.9],'b--','LineWidth',1.5)


function diftab = dif_basis ( ntab, xtab )

%*****************************************************************************80
%
%% DIF_BASIS computes all Lagrange basis polynomials in divided difference form.
%
%  Discussion:
%
%    The I-th Lagrange basis polynomial for a set of NTAB X values XTAB,
%    L(I,NTAB,XTAB)(X) is a polynomial of order NTAB-1 which is zero at
%    XTAB(J) for J not equal to I, and 1 when J is equal to I.
%
%    The Lagrange basis polynomials have the property that the interpolating
%    polynomial through a set of NTAB data points (XTAB,YTAB) may be
%    represented as
%
%      P(X) = Sum ( 1 <= I <= N ) YTAB(I) * L(I,NTAB,XTAB)(X)
%
%    Higher order interpolation at selected points may be accomplished
%    using repeated X values, and scaled derivative values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer NTAB, the number of X data points XTAB, and the number of
%    basis polynomials to compute.
%
%    Input, real XTAB(NTAB), the X values upon which the
%    Lagrange basis polynomials are to be based.
%
%    Output, real DIFTAB(NTAB,NTAB), the set of divided
%    difference tables.  Column I of DIFTAB contains the table for
%    the I-th Lagrange basis polynomial.
%

%
%  Initialize DIFTAB to the identity matrix.
%
  diftab(1:ntab,1:ntab) = 0.0;
  for i = 1 : ntab
    diftab(i,i) = 1.0;
  end
%
%  Compute each Lagrange basis polynomial.
%
  for i = 1 : ntab
    diftab(1:ntab,i) = ( data_to_dif ( ntab, xtab, diftab(1:ntab,i) ) )';
  end

  return
end


function c = r8ut_mxm ( n, a, b )

%*****************************************************************************80
%
%% R8UT_MXM multiplies two R8UT matrices.
%
%  Discussion:
%
%    The R8UT storage format is used for an M by N upper triangular
%    matrix.  The format stores all M*N entries, even those which are zero.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrices.
%    N must be positive.
%
%    Input, real A(N,N), B(N,N), the R8UT factor matrices.
%
%    Output, real C(N,N), the R8UT product matrix.
%
  c = zeros ( n, n );

  for i = 1 : n
   for j = i : n
      for k = i : j
        c(i,j) = c(i,j) + a(i,k) * b(k,j);
      end
    end
  end

  return
end


function [X12,x1,x2] = DigitalOscillator(f1,f2,Fs,t1)

% Function to simulate Digital Sinusodial Oscillator
% Input Variables
%    f1 = first input analog frequency    Suggested Range(697-1477)
%    f2 = second input analog frequency   Suggested Range(697-1477
%    Fs = Sampling Frequency              Default = 8000 cycles/sec
%    t1 = Tone Length                     Default = .1 sec
%
% Output Variables
%    Y  = Sine Signal for two given frequencies
%    x1 = Sine Signal for first frequency
%    x2 = Sine Signal for second frequency
%
%    Rajiv Singla        DSP Final Project            Fall 2005
%=====================================================================

% Checking for minimum number of arguments
if nargin < 2
    error('Not enough input arguments');
end

% Setting Default values
if nargin == 2
    Fs = 8000;
    t1 = 0.1;
end

Ts=1/Fs;   %Sampling Time
samples=t1/Ts;  %Number of samples in the tone

%% Generating first Signal
w0=2*3.1416*f1/Fs;
A=1;
a1=-2*cos(w0);
Y1=0;
Y2=-A*sin(w0);

for n=1:samples
    x1(n)=-a1*Y1-Y2;
    Y2=Y1;
    Y1=x1(n);
end
x1=[0 x1]; %appending zero in the beginning

%% Generating second Signal
w1=2*3.1416*f2/Fs;
A=1;
a2=-2*cos(w1);
P1=0;
P2=-A*sin(w1);

for n=1:samples
    x2(n)=-a2*P1-P2;
    P2=P1;
    P1=x2(n);
end
x2=[0 x2]; %appending zero in the beginning

%% Combining both signal
X12 = .5*(x1 + x2);


function value = pyramid_unit_monomial_3d ( alpha, beta, gamma )

%*****************************************************************************80
%
%% PYRAMID_UNIT_MONOMIAL_3D: monomial integral in a unit pyramid in 3D.
%
%  Discussion:
%
%    This routine returns the integral of X^ALPHA Y^BETA Z^GAMMA over
%    the unit pyramid.
%
%    The unit pyramid is defined as:
%
%    - ( 1 - Z ) <= X <= 1 - Z
%    - ( 1 - Z ) <= Y <= 1 - Z
%              0 <= Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer ALPHA, BETA, GAMMA, the exponents of
%    X, Y and Z in the monomial.
%
%    Output, real PYRAMID_UNIT_MONOMIAL_3D, the volume of the pyramid.
%
  value = 0.0;

  if ( mod ( alpha, 2 ) == 0 & mod ( beta, 2 ) == 0 )

    i_hi = 2 + alpha + beta;

    for i = 0 : i_hi
      value = value + r8_mop ( i ) * r8_choose ( i_hi, i ) / ( i + gamma + 1 );
    end

    value = value * 2.0 / ( alpha + 1 ) * 2.0 / ( beta + 1 );

  end

  return
end


function divdif_test20 ( )

%*****************************************************************************80
%
%% DIVDIF_TEST20 tests DIF_DERIVK_TABLE;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIVDIF_TEST20\n' );
  fprintf ( 1, '  For a divided difference polynomial:\n' );
  fprintf ( 1, '  DIF_DERIVK_TABLE computes the K-th derivative;\n' );
%
%  Set the 0 data points.
%
  n0 = 5;
  x0 = linspace ( -2.0, +2.0, 5 );
%
%  Set data for x^4/24+x^3/3+x^2/2+x+1
%
  f0(1:n0) = 1.0;
  for i = 4 : -1 : 1
    f0(1:n0) = f0(1:n0) .* x0(1:n0) / i + 1.0;
  end
%
%  Compute the difference table.
%
  d0 = data_to_dif ( n0, x0, f0 );
  dif_print ( n0, x0, d0, '  The divided difference polynomial P0:' );

  c0 = dif_to_r8poly ( n0, x0, d0 );

  r8poly_print ( n0, c0, '  Using DIF_TO_R8POLY' );
%
%  Compute the difference table for the K=1 derivative.
%
  k = 1;
  n1 = n0 - k;
  [ x1, d1 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Compute the difference table for the K=2 derivative.
%
  k = 2;
  n2 = n0 - k;
  [ x2, d2 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Compute the difference table for the K=3 derivative.
%
  k = 3;
  n3 = n0 - k;
  [ x3, d3 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Compute the difference table for the K=4 derivative.
%
  k = 4;
  n4 = n0 - k;
  [ x4, d4 ] = dif_derivk_table ( n0, x0, d0, k );
%
%  Evaluate all 5 polynomials.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Evaluate difference tables for the function P0\n' );
  fprintf ( 1, '  and its first four derivatives, P1...P4.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X         P0        P1        P2        P3        P4\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 10
    x = i / 5.0;
    y0 = dif_val ( n0, x0, d0, x );
    y1 = dif_val ( n1, x1, d1, x );
    y2 = dif_val ( n2, x2, d2, x );
    y3 = dif_val ( n3, x3, d3, x );
    y4 = dif_val ( n4, x4, d4, x );
    fprintf ( 1, '  %8.4f  %8.4f  %8.4f  %8.4f  %8.4f  %8.4f\n', ...
      x, y0, y1, y2, y3, y4 );
  end

  return
end


function varargout = PoissonRatio(C,varargin)
% computes the Poisson ratio of an elasticity tensor
%
% Input
%  C - elastic @stiffnessTensor
%  x - @vector3d
%  y - @vector3d
%
% Output
%  nu - Poisson ratio in directions x and y
%
% Description
%
% $$\nu = \frac{-S_{ijkl} x_i x_j y_k y_l}{S_{mnop} x_m x_n x_o x_p}$$
%

% take formula using complience
[varargout{1:nargout}] = PoissonRatio(inv(C),varargin{:});


function loc_3d_torus(NumOfSamples)

% Create Localized 3-d data from uniform distribution.
% Data are bount to the neighborhood near a torus surface.
% Torus surface equation:
%
%         (c - sqrt(x^2+y^2))^2 + z^2 = a^2
%
% This torus is created by revoloution around z axis of a circle centered
% at (c,0) with radious a.

clc;
input_dims = 3;

a = 1;
a1 = .9;
c = 5;
% Initialize Data
Data = [];

while size(Data,2)<NumOfSamples

In1 = 20*(rand(input_dims,1)-.5);

% Apply Mask to input Data
if abs(In1(3,1))<= sqrt(a^2 - (c-sqrt(In1(1,1)^2 + In1(2,1)^2))^2) && ...
                                                                       abs(In1(3,1))>=sqrt(a1^2 - (c-sqrt(In1(1,1)^2 + In1(2,1)^2))^2)
    Data = [Data In1];
end
end

a = randperm(NumOfSamples);
Data = Data(:,a);

save local_torus Data;
plot3(Data(1,:),Data(2,:),Data(3,:),'.','MarkerSize',1);
grid on;
clear all;


function matreg = Tikhonov_rank_def(mat, rank, lambda)

% Apply Tikhonov regularisation to rank-deficient matrix

% mat: square matrix

% rank: number of singular values to be considered in inversion

% lambda: regularisation parameters

% OH, Sep 2018

% SVD of input matrix

[U,S,V] = svd(mat);

% get singular values

s = diag(S);

% take only relevant values

s2 = s(1:rank);

lambda = (lambda/100) * (sum(s2)/rank);

% regularise eigenvalues with Tikhonov and invert

s2 = s2 ./ (s2.^2 + lambda);

% reconstitute regularised inverse matrix

matreg = V(:,1:rank)*diag(s2)*U(:,1:rank)';


function [t,u,v,idx]=raytrace(p0,v0,node,face)
%
% [t,u,v,idx]=raytrace(p0,v0,node,face)
%
% perform a Havel-styled ray tracing for a triangular surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%   p0: starting point coordinate of the ray
%   v0: directional vector of the ray
%   node: a list of node coordinates (nn x 3)
%   face: a surface mesh triangle list (ne x 3)
%
% output:
%   t: signed distance from p to the intersection point for each surface
%      triangle, if ray is parallel to the triangle, t is set to Inf
%   u: bary-centric coordinate 1 of all intersection points
%   v: bary-centric coordinate 2 of all intersection points
%      the final bary-centric triplet is [u,v,1-u-v]
%   idx: optional output, if requested, idx lists the IDs of the face
%      elements that intersects the ray; users can manually calc idx by
%
%      idx=find(u>=0 & v>=0 & u+v<=1.0 & ~isinf(t));
%
% Reference:
%  [1] J. Havel and A. Herout, "Yet faster ray-triangle intersection (using
%          SSE4)," IEEE Trans. on Visualization and Computer Graphics,
%          16(3):434-438 (2010)
%  [2] Q. Fang, "Comment on 'A study on tetrahedron-based inhomogeneous
%          Monte-Carlo optical simulation'," Biomed. Opt. Express, (in
%          press)
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

p0=p0(:)';
v0=v0(:)';

AB=node(face(:,2),1:3)-node(face(:,1),1:3);
AC=node(face(:,3),1:3)-node(face(:,1),1:3);

N=cross(AB',AC')';
d=-dot(N',node(face(:,1),1:3)')';

Rn2=1./sum((N.*N)')';

N1=cross(AC',N')'.*repmat(Rn2,1,3);
d1=-dot(N1',node(face(:,1),1:3)')';

N2=cross(N',AB')'.*repmat(Rn2,1,3);
d2=-dot(N2',node(face(:,1),1:3)')';

den=(v0*N')';
t=-(d+(p0*N')');
P=(p0'*den'+v0'*t')';
u=dot(P',N1')'+den.*d1;
v=dot(P',N2')'+den.*d2;

idx=find(den);
den(idx)=1./den(idx);

t=t.*den;
u=u.*den;
v=v.*den;

 % if den==0, ray is parallel to triangle, set t to infinity
t(find(den==0))=Inf;

if(nargout>=4)
    idx=find(u>=0 & v>=0 & u+v<=1.0 & ~isinf(t));
end


function mean = discrete_mean ( a, b )

%*****************************************************************************80
%
%% DISCRETE_MEAN evaluates the mean of the Discrete PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, the number of probabilities assigned.
%
%    Input, real B(A), the relative probabilities of
%    outcomes 1 through A.  Each entry must be nonnegative.
%
%    Output, real MEAN, the mean of the PDF.
%
  b_sum = sum ( b(1:a) );

  mean = 0.0;
  for j = 1 : a
    mean = mean + j * b(j);
  end

  mean = mean / b_sum;

  return
end


% Maximum volume inscribed ellipsoid in a polyhedron
% Section 8.4.1, Boyd & Vandenberghe "Convex Optimization"
% Original version by Lieven Vandenberghe
% Updated for CVX by Almir Mutapcic - Jan 2006
% (a figure is generated)
%
% We find the ellipsoid E of maximum volume that lies inside of
% a polyhedra C described by a set of linear inequalities.
%
% C = { x | a_i^T x <= b_i, i = 1,...,m } (polyhedra)
% E = { Bu + d | || u || <= 1 } (ellipsoid)
%
% This problem can be formulated as a log det maximization
% which can then be computed using the det_rootn function, ie,
%     maximize     log det B
%     subject to   || B a_i || + a_i^T d <= b,  for i = 1,...,m

% problem data
n = 2;
px = [0 .5 2 3 1];
py = [0 1 1.5 .5 -.5];
m = size(px,2);
pxint = sum(px)/m; pyint = sum(py)/m;
px = [px px(1)];
py = [py py(1)];

% generate A,b
A = zeros(m,n); b = zeros(m,1);
for i=1:m
  A(i,:) = null([px(i+1)-px(i) py(i+1)-py(i)])';
  b(i) = A(i,:)*.5*[px(i+1)+px(i); py(i+1)+py(i)];
  if A(i,:)*[pxint; pyint]-b(i)>0
    A(i,:) = -A(i,:);
    b(i) = -b(i);
  end
end

% formulate and solve the problem
cvx_begin
    variable B(n,n) symmetric
    variable d(n)
    maximize( det_rootn( B ) )
    subject to
       for i = 1:m
           norm( B*A(i,:)', 2 ) + A(i,:)*d <= b(i); %#ok
       end
cvx_end

% make the plots
noangles = 200;
angles   = linspace( 0, 2 * pi, noangles );
ellipse_inner  = B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );
ellipse_outer  = 2*B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );

clf
plot(px,py)
hold on
plot( ellipse_inner(1,:), ellipse_inner(2,:), 'r--' );
plot( ellipse_outer(1,:), ellipse_outer(2,:), 'r--' );
axis square
axis off
hold off


function fx = p42_fun ( n, x )

%*****************************************************************************80
%
%% P42_FUN evaluates the integrand for problem 42.
%
%  Discussion:
%
%    The problem has a parameter ALPHA that can be set by calling
%    P42_PARAM_SET.
%
%    The integrand has a singularity at X = 0 if ALPHA < 1.
%
%    The suggested range for ALPHA is 0.1 through 2.
%
%  Interval:
%
%    0 <= x <= pi/2
%
%  Integrand:
%
%    ( sin(x) )^( alpha - 1 )
%
%  Exact Integral:
%
%    2^( alpha - 2 ) * ( Gamma(alpha/2) )^2 / Gamma(alpha)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Piessens, Elise de Doncker-Kapenga,
%    Christian Ueberhuber, David Kahaner,
%    QUADPACK: A Subroutine Package for Automatic Integration,
%    Springer, 1983, page 84.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  alpha = p42_param_get ( );

  fx = sin ( x ).^( alpha - 1.0 );

  if ( alpha < 1.0 )
    i = find ( x == 0.0 );
    fx(i) = 0.0;
  end

  return
end


function c=addlogs(a,b)
%ADDLOGS Add numbers represented by their logarithms.
%
%  Description
%    C=ADDLOGS(A,B) computes C=log(exp(A)+exp(B)) in such a fashion
%    that it works even when A and B have large magnitude.

% Copyright (c) 2003 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.

if a>b
  c = a + log(1+exp(b-a));
else
  c = b + log(1+exp(a-b));
end


function [Gausspoint,Gaussweight] = GaussQuadrature(ngl)
%-------------------------------------------------------------------
%  Purpose:
%     determine the integration points and weighting coefficients
%     of Gauss-Legendre quadrature for two-dimensional integration
%
%  Synopsis:
%     [point,weight]=GaussQuadrature(nglx,ngly)
%
%  Variable Description:
%     ngl - number of integration points
%     point - vector containing integration points
%     weight - vector containing weighting coefficients
%-------------------------------------------------------------------
%  initialization

   Gausspoint=zeros(ngl,1);
   Gaussweight=zeros(ngl,1);

%  corresponding integration points and weights
    % 2-point quadrature rule
    Gausspoint(1)=-0.577350269189626;
    Gausspoint(2)=-Gausspoint(1);
    Gaussweight(1)=1.0;
    Gaussweight(2)=Gaussweight(1);


function [ R ] = R_rpy( roll, pitch, yaw )
%R_RPY この関数の概要をここに記述
%   Camera rotates (1) yaw (y-axis) (2)pitch (x'-axis) (3)roll (z''-axis)

rrad = -roll * pi / 180.0;
prad = -pitch * pi / 180.0;
yrad = -yaw * pi / 180.0;

Rr = [cos(rrad), -sin(rrad), 0;...
      sin(rrad), cos(rrad), 0;...
      0, 0, 1];

Rp = [1, 0, 0;...
      0, cos(prad), -sin(prad);...
      0, sin(prad), cos(prad)];

Ry = [cos(yrad), 0, sin(yrad);...
      0, 1, 0;...
      -sin(yrad), 0, cos(yrad)];

R = Rr * Rp * Ry;

end


function s = CubicTimeScaling(Tf, t)
% *** CHAPTER 9: TRAJECTORY GENERATION ***
% Takes Tf: Total time of the motion in seconds from rest to rest,
%       t: The current time t satisfying 0 < t < Tf.
% Returns s: The path parameter s(t) corresponding to a third-order
%            polynomial motion that begins and ends at zero velocity.
% Example Input:
%
% clear; clc;
% Tf = 2;
% t = 0.6;
% s = CubicTimeScaling(Tf,t)
%
% Output:
% s =
%    0.2160

s = 3 * (t / Tf) ^ 2 - 2 * (t / Tf) ^ 3;
end


function transformationMatrix = create_transformation_matrix3d(varargin)
% CREATE_TRANSFORMATION_MATRIX3D Create a 3D transformation matrix
%
% [transformationMatrix rotationMatrix shearMatrix scaleMatrix] = ...
%   create_transformation_matrix3d()
%
% INPUT ARGUMENTS
% N/A
%
% Optional input arguments
% 'phiX'            - Rotation angle around x-axis
% 'phiY'            - Rotation angle around y-axis
% 'phiZ'            - Rotation angle around z-axis
% 'shearXY'         - Shear XY
% 'shearXZ'         - Shear XZ
% 'shearYX'         - Shear YX
% 'shearYZ'         - Shear YZ
% 'shearZX'         - Shear ZX
% 'shearZY'         - Shear ZY
% 'scaleX'          - Scale along x-axis
% 'scaleY'          - Scale along y-axis
% 'scaleZ'          - Scale along z-axis
% 'translationX'    - Translation along x-axis
% 'translationY'    - Translation along y-axis
% 'translationZ'    - Translation along z-axis
%
% OUTPUT ARGUMENTS
% transformationMatrix  - Total transformation matrix
% rotationMatrix        - Rotation matrix
% shearMatrix           - Shear matrix
% scaleMatrix           - Scale matrix

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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/>.

phiX = 0;
phiY = 0;
phiZ = 0;
shearXY = 0;
shearXZ = 0;
shearYX = 0;
shearYZ = 0;
shearZX = 0;
shearZY = 0;
scaleX = 1;
scaleY = 1;
scaleZ = 1;
translationX = 0;
translationY = 0;
translationZ = 0;

for k=1:2:length(varargin),         % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

rotationX = [1 0         0;
             0 cos(phiX) -sin(phiX);
             0 sin(phiX) cos(phiX)];
rotationY = [cos(phiY)  0 sin(phiY);
             0          1 0;
             -sin(phiY) 0 cos(phiY)];
rotationZ = [cos(phiZ) -sin(phiZ) 0;
             sin(phiZ) cos(phiZ)  0;
             0         0          1];

shearMatrix = [1       shearXY shearXZ;
               shearYX 1       shearYZ;
               shearZX shearZY 1];

scaleMatrix = [scaleX 0      0;
               0      scaleY 0;
               0      0      scaleZ];

transformationMatrix = zeros(3,4);
transformationMatrix(1:3,1:3) = rotationX*rotationY*rotationZ*shearMatrix*scaleMatrix;
transformationMatrix(:,4) = [translationX translationY translationZ]';


function [p,c,v] = matrixpolynomial(x,n,dmax,dmin)
%MATRIXPOLYNOMIAL Creates parameterized polynomial
%
% [p,c,v] = matrixpolynomial(x,n,dmax,dmin)
%
% MATRIXPOLYNOMIAL is a quick way to define a parameterized polynomial
% p=sum C_i v_i(x) with all monomials of dmin <= degree(p,x) <= dmax. The
% coefficients in the polynomial are C_i while v is the monomial basis.
%
% Example:
%
% Paramterized quartic 2x2 matrix
%  x = sdpvar(2,1);
%  p = matrixpolynomial(x,2,4);
%
% See also MONOLIST, COEFFICIENTS

if (length(dmax) > 1) && (length(dmax) ~= length(x))
    error('Dimension mismatch: The third argument should be the max degree for each variable, or a sclar');
end

if nargin > 3
    if (length(dmin) > 1) && (length(dmin) ~= length(x))
        error('Dimension mismatch: The third argument should be the max degree for each variable, or a sclar');
    end
end

if any(dmax < 0)
    error('Only non-negative polynomial degrees possible')
end

if nargin<4
    dmin = 0;
end

if any(dmin > dmax)
    error('Fourth argument (dmin) should not be larger than second argument (dmax)');
end

if any(dmin < 0)
    error('Only non-negative polynomial degrees possible')
end

if length(n)==1
    n = [n n];
end

v = monolist(x,dmax);

if dmin <= dmax & dmin>0
    s = nchoosek(length(x) + dmin-1,dmin-1);
    v = extsubsref(v,s+1:length(v));
end

p = 0;
c = [];
for i = 1:length(v)
    Ci = sdpvar(n(1),n(2));
    c = [c reshape(Ci,[],1)];
end
p = reshape(c*v,n(1),n(2));


%% Example:
% Compute the tridiagonal matrix used to solve implicitly a laplace (heat)
% equation.

m = 5; % domain size.
h = 0.1; % delta x value.

I = eye(m);
e = ones(1,m);
x = spdiags([e' -4*e' e'],[-1 0 1],m,m);
%x = zeros(m);
%x = x+spdiags([e' -4*e' e'],[-1 0 1],m,m);
s = spdiags([e',e'],[-1 1],m,m);
s = zeros(m);
s = s+spdiags([e',e'],[-1 1],m,m);
%kron(I,x);
%kron(s,I);
A = (kron(I,x)+kron(s,I))/h^2;
%size(A);


% GENERATE BILINEAR TIME-FREQUENCY TRANSFORMATIONS USING THE DIRECT METHOD
%
% BILINEAR TRANSFORMATIONS transform the time domain signal (the
% variable INPUT SIGNAL) to an output time-frequency distribution (the
% variable OUTPUT TIME-FREQUENCY ARRAY)
%
% These functions use a direct implementation rather than using the
% quadratic time-frequency implementation. This results in a
% computationally optimised routine.
%
% The different types of time-frequency distributions that can be
% implemented directly are:
%
%   (i)    Wigner-Ville Distribution
%   (ii)   Short-Time Fourier Transform
%   (iii)  Short-Time Fourier Transform(overlap)
%   (iv)   Rihaczek
%   (v)    Windowed-Rihaczek
%
%   The various parameters associated with the distributions are as follows:
%
%   TIME-FREQUENCY ARRAY (tfd)
%
%      The computed time-frequency distribution.  size(tfd) will
%      return [a, b], where a is the next largest power of two above
%      FFT length, and b is floor(length(signal)/time_res) - 1.
%
%   INPUT SIGNAL
%
%      Input one dimensional signal to be analysed. An analytic signal
%      is required for this function, however, if signal is real, a
%      default analytic transformer routine will be called from this
%      function before computing tfd.
%
%   TIME RESOLUTION
%
%      The number of time samples to skip between successive time slices.
%
%   LAG WINDOW LENGTH
%
%      This is the lag window length and controls the size of the
%      signal kernel (or instantaneous autocorrelation function) used
%      for analysis (lag_window_length must be odd). The kernel used
%      will be defined from -(lag_window_length+1)/2 to
%      +(lag_window_length+1)/2 in both time and lag dimensions.
%
%   FFT Length:
%
%      Zero-padding at the FFT stage of the analysis may be specified
%      by giving an FFT length larger than lag window length.  If
%      FFT length is not specified, or is smaller than the
%      lag window length, then the next highest power of two above
%      lag window length is used.  If FFT length is not a power of
%      two, the next highest power of two is used.
%
%
%
%  See Also:  wvd, spec, rihaczek, analyt


function [c4,m4,c2] = cum4(X,prewhiten)
%CUM4 Fourth-order cumulant tensor.
%   [c4,m4,c2] = cum4(X) computes the second-order cumulant (covariance
%   matrix) c2, fourth-order moment m4 and fourth-order cumulant
%   (quadricovariance tensor) c4 of a matrix X in which each row is an
%   observation and each column is a variable. Herein,
%
%      c2(i,j)     = E[xi.*conj(xj)]
%      m4(i,j,k,l) = E[xi.*conj(xj).*conj(xk).*xl]
%      c4(i,j,k,l) = E[xi.*conj(xj).*conj(xk).*xl] ...
%                    - E[xi.*conj(xj)]*E[conj(xk).*xl] ...
%                    - E[xi.*conj(xk)]*E[conj(xj).*xl] ...
%                    - E[xi.*xl]*E[conj(xj).*conj(xk)]
%
%   where the expectation E is approximated by the arithmetic mean and xi
%   is the i-th mean centered variable, X(:,i)-mean(X(:,i)) (and
%   analogously for xj, xk and xl).
%
%   [c4,m4,c2] = cum4(X,'prewhiten') applies a linear transformation to the
%   columns of X so that the covariance matrix of the new matrix is the
%   identity matrix before computing its fourth-order cumulant.
%
%   See also cov, scov.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] P. McCullagh, "Tensor Methods in Statistics," Chapman and Hall,
%       London, 1987.
%   [2] C. Nikias, A. Petropulu, "Higher-Order Spectra Analysis: A
%       Nonlinear Signal Processing Framework," Prentice Hall, 1993.

% Check the prewhiten option.
if nargin < 3, prewhiten = false; end
if ischar(prewhiten), prewhiten = strcmpi(prewhiten,'prewhiten'); end

% Center the variables.
X = bsxfun(@minus,X,mean(X));

% Apply a prewhitening to X if requested.
n = size(X,1);
if prewhiten
    [U,S,~] = svd(X,'econ');
    X = U*(S*pinv(S))*sqrt(n);
end

% Compute c2 = E[xi*conj(xj)] and r2 = E[xi*xj].
c2 = conj(X'*X)/n;
r2 = (X.'*X)/n;

% Compute m4 = E[xi*conj(xj)*conj(xk)*xl].
% Introduce singleton dimensions 3 and 4.
m4 = bsxfun(@times,permute(X,[2 3 4 5 1]),permute(conj(X),[3 2 4 5 1]));
m4 = mean(bsxfun(@times,m4,permute(m4,[3 4 2 1 5])),5);

% Compute c4(i,j,k,l) = m4 - E[xi.*conj(xj)]*E[conj(xk).*xl] - ...
% E[xi.*conj(xk)]*E[conj(xj).*xl] - E[xi.*xl]*E[conj(xj).*conj(xk)].
c4 = m4-bsxfun(@times,c2,permute(c2,[3 4 2 1])) ...
       -bsxfun(@times,permute(c2,[1 3 2 4]),permute(c2,[3 2 4 1])) ...
       -bsxfun(@times,permute(r2,[1 3 4 2]),permute(conj(r2),[3 1 2 4]));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) Exercise A: Use the Runge-Kutta 4 algorithm to integrate the
%   differential equation:
%            dy/dt = 2*t
%      Integrate from t0=0, to tfinal = 10 s.
%      Compare the solution with the algebraic integration.
%   B) Exercise B: Use the Runge-Kutta 4 algorithm to integrate the
%   second order differential equation:
%            d2y/dt^2+5*dy/dt + 4*y(t) = 0
%
%   C) Exercise C: Use the Runge-Kutta 4 algorithm to simulate the movement
%   of a 1 dof robot arm with friction under the effect of gravity and with
%   zero torque applied.
%
%   D) Exercise D: Use the Runge-Kutta 4 algorithm to simulate the movement
%   of a 2 dof robot arm with friction under the effect of gravity and with
%   zero torques applied.
%
% Help: Function prototype
% [y, t] = runge_kutta(f, y0, [t0 tfinal], timestep)
% where f is the function being integrated as dy/dt = f(t, y).
% y0 are the initial conditions
% t0: initial time
% tfinal: final time.
% h: time step for the calculations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function runge_kutta_exercises()
close all;

%uncomment to execute each of the exercises
% exerciseA()%
exerciseB()
%exerciseC()
%exerciseD()

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Integrate a simple time function. dy/dt = 2*ts
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseA()
t0 = 0;
tfinal = 10;
[t, y] = runge_kutta(@line, 0, [t0 tfinal], 1);

%Compare with the integral of 2*t
error = y(:)-t(:).^2;
mean(error)
plot(t, y)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use Runge-Kutta to integrate a second order equation of the form.
% d2y/dt^2+5*dy/dt + 4*y(t) = 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseB()
t0 = 0;
tfinal = 10;
%                                          initial values
[t, y] = runge_kutta(@second_order_system, [10 1]', [t0 tfinal], 0.01);

y = y(:, 1:length(t));
%plot results
plot(t, y(1,:)), hold
plot(t, y(2,:)), hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now use Runge-kutta to integrate the movement of a 1 dof robot arm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseC()
%these variables are shared by the forward_dynamic_robot1 function defined
%below
global robot tau g
t0 = 0;
tfinal = 10;
robot = load_robot('example','1dofplanar')
robot.dynamics.friction=0
tau = [0];
g = [0 -9.81 0]';
[t, y] = runge_kutta(@forward_dynamic_robot1, [0 0]', [t0 tfinal], 0.01);

figure, plot(y')
% Animate the movement. Change speed from 1-30-100-200
speed = 5
animate(robot,[y(1,1:speed:length(y)); y(2,1:speed:length(y))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now use Runge-Kutta to simulate the movement of a 2 DOF robot arm.
function exerciseD()
global robot tau g
t0 = 0;
tfinal = 10;
tau = [0 0];
g = [0 -9.81 0]';
robot = load_robot('example','2dofplanar')
robot.dynamics.friction=1

[t, y] = runge_kutta(@forward_dynamic_robot2, [0 0 0 0]', [t0 tfinal], 0.01);

%speed, change to 10, 30, 50, 100
speed = 5
animate(robot,[y(1,1:speed:length(y)); y(2,1:speed:length(y))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function.
% The function returns dy/dt = 2*t. Function called from exerciseA()
%
% Integrate a line in time. dy/dt = 2*t. Obviously, the integration should
% yield. y(t) = t^2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dy = line(t, y)
dy = 2*t;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to solve a second order differential equation.
% Called from function exerciseB()
%
% In order to solve for d2y/dt^2 + 5*dy/dt + 4*y(t) = 0. We can use:
% x1 = y
% x2 = dy/dt
% thus,
% dx1/dt = x2
% dx2/dt = d2y/dt^2=-5*dy/dt-4*y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = second_order_system(t, y)
% We must return the solution of
% [dx1/dt; dx2/dt]
xd(1) = y(2);
xd(2) = 0 - 5*y(2)-4*y(1);
xd = xd(:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to simulate the movement of a 1 DOF robot
% Called from function exerciseC()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = forward_dynamic_robot1(t, y)
global tau g robot

qdd = accel(robot, y(1), y(2), tau, g);
%return qd, qdd
xd = [y(2); qdd];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to simulate the movement of a 2 DOF robot
% Called from function exerciseD()
% t is only used to plot time on screen during simulation.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = forward_dynamic_robot2(t, y)
global tau g robot
%we must return the solution of
% [dx1/dt; dx2/dt]
t
% caution, g in the forwarddynamics_2dofplanar takes the opposite sign
qdd=forwarddynamics_2dofplanar(robot, y(1:2,1), y(3:4,1), tau', -sum(g), [0 0 0 0 0 0]);
xd = [y(3:4,1); qdd];


function [phi_g, ip, x_g,  w_g] = boundary(i_bnd, x_local, n_gauss)

% Compute values of 2D quadratic bases at 1D Gauss points along a boundary
% Also return a list of (3) indices for the non-zero basis functions,
% the co-ords of the Gauss points and the 1D integration weights
%
% i_bnd specifies the triangle boundary of interest
%       1 - along s   = 0 boundary   - points [1 4 2]
%       2 - along r+s = 1 boundary   - points [2 5 3]
%       3 - along r   = 0 boundary   - points [3 6 1]
% x_local 6 x 2 array of nodal coordinate points (x, y)
% n_gauss  the number of Gauss points for the 1D integration

% phi_g is a n_gauss array x 3 - values of the 3 non-zero quadratic basis
%                                functions at the n_gauss 1D integration points
% ip  array of indices for non-zero quadratic functions
% x_g   is an n_gauss x 2 array of coordinates for the 1D Gauss points
% w_g   is an n_gauss array of integration weights

%% extract the Gauss weights and points on [- 1, 1];
     [r,  w_g] = oned_gauss(n_gauss); % 1 \le n_gauss \le 6

     r1 = (1+r)/2; % map to [0, 1]
     r1 = r1(:);   % make sure it's a column

%% which boundary is requested
     switch i_bnd
         case {1}
             ip = [ 1 4 2];
         case {2}
             ip = [ 2 5 3];
         case {3}
             ip = [ 3 6 1];
         otherwise
             disp(['Illegal i_bnd = ', int2str(i_bnd)])
             return
     end

%% Coordinates of the Gauss points
     x_g = (1-r1)*x_local(ip(1),:) ...
          +   r1 *x_local(ip(3),:);

%% Scale the weights by the (unsigned) length of the interval
     djac  =  norm(x_local(ip(3), :) - x_local(ip(1), :))/2;
     w_g   =  w_g*djac;

%% Evaluate the test functions

      p1 = 1 -3*r1 + 2*r1.^2; % p1(r = 0)  = 1
      p2 =    4*r1 - 4*r1.^2; % p2(r = 1/2)= 1
      p3 =   -  r1 + 2*r1.^2; % p3(r = 1)  = 1

      phi_g = [p1(:)  p2(:) p3(:) ];

  return
end


function [y,exitCode]=geometricMedian(x,eta,AbsTol,RelTol,maxIter,test4Duplicates)
%%GEOMETRICMEDIAN Compute the weighted geometric median of the (possibly
%          multivariate) points x. This finds the vector y that minimizes
%          sum_i eta(i)*norm(y-x(:,i),1) where all eta(i)>0.
%          The geometric median is also known as the L1-median and the
%          spatial median.
%
%INPUTS: x The xDimXN set of N real vectors whose geometric median is
%          desired.
%      eta The NX1 or 1XN set of all positive weights. If this parameter is
%          omitted or an empty matrix is passed, then uniform weights are
%          used.
% AbsTol, RelTol Absolute and relative tolerances for determining
%          convergence. If diff is the difference between the estimate at
%          the current and the previous iteration, then convergence is
%          declared if diff<AbsTol||diff<RelTol*norm(y). These same
%          tolerances are used in a similar manner to determine whether y
%          equals any of the points in x during an iterative update and
%          whether any of the vectors in x are duplicates of each other.
%          The defaults if omitted or empty matrices are passed are
%          RelTol=1e-9, AbsTol=1e-12.
%  maxIter The maximum number of iterations to perform. The default if
%          ommitted or an empty matrix is passed is max(1000,20*xDim).
% test4Duplicates The algorithms assume that none of the vectors in x are
%          duplicated of each other. test4Duplicates can be set to false if
%          it is known that there are no duplicated vectors in x. The
%          default if omitted or an empty matrix is passed is true.
%
%OUTPUTS: y The xDimX1 geometric median of the vectors in x.
%  exitCode A value indicating how the iterative algorithm terminated.
%           Possible values are:
%           0 The algorithm converged.
%           1 The algorithm did not convergence withing maxiter iteration.
%
%This function uses the modified Weiszfeld algorithm, which is described in
%[1]. The algorithm is inistialized with the Convergence is linear. A
%quasi-Newton algorithm would presumably be better for very large problems.
%
%EXAMPLE:
%Here, we have a set of data with an outlier. one will see that the mean is
%pulled towards the outlier, whereas the geometric median is more robust.
% x=[-26,   5,   1,  -2,  13,  -1,   0,   3,  -2, -12,  120;
%      0,  12,  12,  19,  -8, -16,  -5,   3,   4,   6,  100];
% yGM=geometricMedian(x);
% yM=mean(x,2);
% costGM=sum(sqrt(sum((bsxfun(@minus,yGM,x)).^2,1)))
% costM=sum(sqrt(sum((bsxfun(@minus,yM,x)).^2,1)))
% figure(1)
% clf
% hold on
% scatter(x(1,:),x(2,:),'ok')
% scatter(yGM(1),yGM(2),'r','filled')
% scatter(yM(1),yM(2),'b','filled')
% legend('Points','Geometric Median','Mean')
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] Y. Vardi and C.-H. Zhang, "The multivariate l1-median and associated
%    data depth," Proceedings of the National Academy of Sciences of the
%    United States of America, vol. 97, no. 4, pp. 1423-1426, 23 Feb. 2000.
%
%August 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(x,2);
xDim=size(x,1);

if(nargin<6||isempty(test4Duplicates))
    test4Duplicates=true;
end

if(nargin<5||isempty(maxIter))
    maxIter=max(1000,20*xDim);
end

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

if(nargin<3||isempty(AbsTol))
    AbsTol=1e-12;
end

if(nargin<2||isempty(eta))
    eta=ones(1,n);
end

%Get rid of duplicate values and adjust the weights.
if(test4Duplicates)
    xUnique=zeros(xDim,n);
    xUnique(:,1)=x(:,1);
    etaUnique=zeros(1,n);
    etaUnique(1)=eta(1);
    numUnique=1;

    xMag=sqrt(sum(x.*x,1));
    for curX=2:n

        d=x(:,curX)-xUnique(:,1:numUnique);
        dMag=sqrt(sum(d.^2,1));
        idx=find(dMag<AbsTol|dMag<RelTol*xMag(curX),1);

        if(isempty(idx))%It is unique.
          numUnique=numUnique+1;
          xUnique(:,numUnique)=x(:,curX);
          etaUnique(numUnique)=eta(curX);
        else%It is not unique.
            etaUnique(idx)=etaUnique(idx)+eta(curX);
        end
    end

    x=xUnique;
    eta=etaUnique;
    n=numUnique;
end

%The special case when given a single value.
if(n==1)
  y=x;
  exitCode=0;
  return
end

eta=eta(:).';
xMag=sqrt(sum(x.*x,1));%Magnitude

%Start with the weighted mean as the initial estimate.
y=sum(bsxfun(@times,(eta/sum(eta)),x),2);

exitCode=1;
for curIter=1:maxIter
  yPrev=y;

  %Duplicates were removed (weights combined) prior to beginning iterations, so
  %there should be at most 1 point coincident with x. We will use absolute and
  %relative tolerances to determine equality.
  d=bsxfun(@minus,y,x);
  dMag=sqrt(sum(d.^2,1));
  idx=find(dMag<AbsTol|dMag<RelTol*xMag,1);

  if(isempty(idx))
    %If no points coincide with x, then use Weiszfeld's original iteration.

    %Defined after Equation 2.3
    w=eta./dMag;
    w=w/sum(w);

    %Equation 2,3
    y=sum(bsxfun(@times,w,x),2);
  else
    %If any point in x coincides with y, then use the modified Weiszfeld
    %iteration.

    %Equation 2,5. The only nonzero weight is the one  where the values
    %coincide.
    etay=eta(idx);

    %w as in Equation 23, but not normalized.
    w=eta./dMag;
    w(idx)=0;
    %The normalization constant.
    wNorm=sum(w);

    %Equation 2,4
    Ty=sum(bsxfun(@times,(w/wNorm),x),2);

    %Equation 2,7
    ry=norm(sum(bsxfun(@times,w,-d),2),1);

    %Equation 2.6
    y=max(0,(1-etay/ry))*Ty+min(1,etay/ry)*y;
  end

  yNorm=norm(y);
  diff=norm(y-yPrev);

  if(diff<AbsTol||diff<RelTol*yNorm)
      exitCode=0;
      break;
  end
end

end

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


function value = r8_cinh ( x )

%*****************************************************************************80
%
%% R8_CINH: alternate hyperbolic cosine integral Cinh of an R8 argument.
%
%  Discussion:
%
%    Cinh ( x ) = Integral ( 0 <= t <= x ) ( cosh ( t ) - 1 ) dt / t
%
%    The original text of this program had a mistake:
%      y = x * x / 9.0 - 1.0
%    has been corrected to
%      y = x * x / 4.5 - 1.0
%    JVB, 27 March 2010
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the hyperbolic cosine integral Cinh
%    evaluated at X.
%
  persistent cinhcs
  persistent eul
  persistent ncinh
  persistent xmin
  persistent xsml

  eul = 0.57721566490153286060651209008240;

  if ( isempty ( ncinh ) )

    cinhcs = [ ...
      0.1093291636520734431407425199795917, ...
      0.0573928847550379676445323429825108, ...
      0.0028095756978830353416404208940774, ...
      0.0000828780840721356655731765069792, ...
      0.0000016278596173914185577726018815, ...
      0.0000000227809519255856619859083591, ...
      0.0000000002384484842463059257284002, ...
      0.0000000000019360829780781957471028, ...
      0.0000000000000125453698328172559683, ...
      0.0000000000000000663637449497262300, ...
      0.0000000000000000002919639263594744, ...
      0.0000000000000000000010849123956107, ...
      0.0000000000000000000000034499080805, ...
      0.0000000000000000000000000094936664, ...
      0.0000000000000000000000000000228291, ...
      0.0000000000000000000000000000000484 ]';

    ncinh = r8_inits ( cinhcs, 16, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( r8_mach ( 3 ) );
    xmin = 2.0 * sqrt ( r8_mach ( 1 ) );

  end

  absx = abs ( x );

  if ( x == 0.0 )
    value = 0.0;
  elseif ( absx <= xmin )
    value = 0.0;
  elseif ( x <= xsml )
    y = - 1.0;
    value = x * x * ( 0.25 + r8_csevl ( y, cinhcs, ncinh ) );
  elseif ( x <= 3.0 )
    y = x * x / 4.5 - 1.0;
    value = x * x * ( 0.25 + r8_csevl ( y, cinhcs, ncinh ) );
  else
    value = r8_chi ( absx ) - eul - log ( absx );
  end

  return
end


function value = r8_atan ( y, x )

%*****************************************************************************80
%
%% R8_ATAN computes the inverse tangent of the ratio Y / X.
%
%  Discussion:
%
%    R8_ATAN returns an angle whose tangent is ( Y / X ), a job which
%    the built in functions ATAN and ATAN2 already do.
%
%    However:
%
%    * R8_ATAN always returns a positive angle, between 0 and 2 PI,
%      while ATAN and ATAN2 return angles in the interval [-PI/2,+PI/2]
%      and [-PI,+PI] respectively;
%
%    * R8_ATAN accounts for the signs of X and Y, (as does ATAN2).  The ATAN
%     function by contrast always returns an angle in the first or fourth
%     quadrants.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Y, X, two quantities which represent the tangent of
%    an angle.  If Y is not zero, then the tangent is (Y/X).
%
%    Output, real VALUE, an angle between 0 and 2 * PI, whose tangent is
%    (Y/X), and which lies in the appropriate quadrant so that the signs
%    of its cosine and sine match those of X and Y.
%

%
%  Special cases:
%
  if ( x == 0.0 )

    if ( 0.0 < y )
      value = pi / 2.0;
    elseif ( y < 0.0 )
      value = 3.0 * pi / 2.0;
    elseif ( y == 0.0 )
      value = 0.0;
    end

  elseif ( y == 0.0 )

    if ( 0.0 < x )
      value = 0.0;
    elseif ( x < 0.0 )
      value = pi;
    end
%
%  We assume that ATAN2 is correct when both arguments are positive.
%
  else

    abs_y = abs ( y );
    abs_x = abs ( x );

    theta_0 = atan2 ( abs_y, abs_x );

    if ( 0.0 < x && 0.0 < y )
      value = theta_0;
    elseif ( x < 0.0 && 0.0 < y )
      value = pi - theta_0;
    elseif ( x < 0.0 && y < 0.0 )
      value = pi + theta_0;
    elseif ( 0.0 < x && y < 0.0 )
      value = 2.0 * pi - theta_0;
    end

  end

  return
end


function [E,GReuss,GHill] = shearModulus(S,h,u)
% shear modulus for an compliance tensor
%
% Syntax
%
%   [GV,GR,GVRH] = shearModulus(S) % the isotropic case
%
%   E = shearModulus(S,h,u) % the anisotropic case with plane h and shear direction  u
%   E = shearModulus(S,[],u)
%   E = shearModulus(S,h,[])
%
% Input
%  C - elastic @stiffnessTensor
%  h - shear plane @vector3d
%  u - shear direction @vector3d
%
% Output
%  E - shear modulus
%  GVoigt - Voigt effective shear modulus, upper bound
%  GReuss - Reuss effective shear modulus, lower bound
%  GHill - Hill effective shear modulus
%
% Description
%
% $$E = \frac{1}{4 S_{ijkl} h_i u_j h_k u_l}$$
%
% See also
% complianceTensor/YoungsModulus complianceTensor/volumeCompressibility complianceTensor/ChristoffelTensor

if nargin == 1 % the isotropic case

  % compute stifness tensor as 6x6 matrices
  C = matrix(inv(S),'voigt');
  S = matrix(S,'voigt');

  % the Voigt upper bound
  % GV = ((C(1,1)+C(2,2)+C(3,3))-(C(1,2)+C(2,3)+C(3,1))+3*(C(4,4)+C(5,5)+C(6,6)))/15
  GVoigt = ((C(1,1,:) + C(2,2) + C(3,3,:)) ...
    - (C(1,2,:) + C(2,3,:) + C(3,1,:)) ...
    + 3 * (C(4,4,:) + C(5,5,:) + C(6,6,:))) ./ 15;

  % the Reuss lower bound
  % GR = 15/(4*(S(1,1)+S(2,2)+S(3,3))-4*(S(1,2)+S(2,3)+S(3,1))+3*(S(4,4)+S(5,5)+S(6,6)))
  GReuss = 15 ./ (4 * (S(1,1,:) + S(2,2,:) + S(3,3,:)) ...
    - 4 * (S(1,2,:) + S(2,3,:) + S(3,1,:)) ...
    + 3 * (S(4,4,:) + S(5,5,:) + S(6,6,:)));

  % Voigt Reuss Hill average
  GHill = 0.5.*(GVoigt + GReuss);
  E = GVoigt;

elseif nargin == 2 || isempty(h)

  E = S2FunHarmonicSym.quadrature(@(u) shearModulus(S,h,u),'bandwidth',4,S.CS);

elseif isempty(u)

  E = S2FunHarmonicSym.quadrature(@(u) shearModulus(S,h,u),'bandwidth',4,S.CS);

else

  % the anisotropic shear modulus
  E = 0.25./EinsteinSum(S,[-1 -2 -3 -4],h,-1,u,-2,h,-3,u,-4);

end


function cdf = hypergeometric_cdf ( x, n, m, l )

%*****************************************************************************80
%
%% HYPERGEOMETRIC_CDF evaluates the Hypergeometric CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X, the argument of the CDF.
%
%    Input, integer N, the number of balls selected.
%    0 <= N <= L.
%
%    Input, integer M, the number of white balls in the population.
%    0 <= M <= L.
%
%    Input, integer L, the number of balls to select from.
%    0 <= L.
%
%    Output, real CDF, the value of the CDF.
%
  c1_log = binomial_coef_log ( l - m, n );
  c2_log = binomial_coef_log ( l, n );

  pdf = exp ( c1_log - c2_log );
  cdf = pdf;

  for x2 = 0 : x - 1

    pdf = pdf * ( m - x2 ) * ( n - x2 ) ...
      / ( ( x2 + 1 ) * ( l - m - n + x2 + 1 ) );

    cdf = cdf + pdf;

  end

  return
end


function oeprint(mu, oev)

% print six classical orbital elements
% (orbital period in days, sma in kilometers)

% input

%  mu      = gravitational constant (km^3/sec^2)
%  oev(1)  = semimajor axis (km)
%  oev(2)  = orbital eccentricity (non-dimensional)
%            (0 <= eccentricity < 1)
%  oev(3)  = orbital inclination (radians)
%            (0 <= inclination <= pi)
%  oev(4)  = argument of periapsis (radians)
%            (0 <= argument of periapsis <= 2 pi)
%  oev(5)  = right ascension of ascending node (radians)
%            (0 <= raan <= 2 pi)
%  oev(6)  = true anomaly (radians)
%            (0 <= true anomaly <= 2 pi)

% Orbital Mechanics with MATLAB

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

rtd = 180 / pi;

% unload orbital elements array

sma = oev(1);
ecc = oev(2);
inc = oev(3);
argper = oev(4);
raan = oev(5);
tanom = oev(6);

arglat = mod(tanom + argper, 2.0 * pi);

period = 2.0 * pi * sma * sqrt(sma / mu) / 86400.0;

% print orbital elements

fprintf ('\n      sma (km)        eccentricity     inclination (deg)    argper (deg)');

fprintf ('\n %12.10e  %12.10e  %12.10e  %12.10e \n', sma, ecc, inc * rtd, argper * rtd);

fprintf ('\n     raan (deg)     true anomaly (deg)   arglat (deg)       period (days)');

fprintf ('\n %12.10e  %12.10e  %12.10e  %12.10e \n', raan * rtd, tanom * rtd, arglat * rtd, period);


function [v,lambda] = trideigs ( a, b, TOL, Nmax, vects )

%QRST           determine all of the eigenvalues (and optionally all of the
%               eigenvectors) of a symmetric tridiagonal matrix using the
%               QR algorithm with Wilkinson shift
%
%     calling sequences:
%             [lambda, v] = qrst ( a, b, TOL, Nmax, vects )
%             [lambda, v] = qrst ( a, b, TOL, Nmax )
%             lambda = qrst ( a, b, TOL, Nmax )
%             qrst ( a, b, TOL, Nmax, vects )
%             qrst ( a, b, TOL, Nmax )
%
%     inputs:
%             a       vector containing elements along the main diagonal
%                     of the symmetric tridiagonal matrix whose eigenvalues
%                     are to be determined
%             b       vector containing elements along the off diagonal
%                     of the symmetric tridiagonal matrix whose eigenvalues
%                     are to be determined
%             TOL     convergence tolerance
%             Nmax    maximum number of iterations
%             vects   optional input argument
%                     matrix containing eigenvector information produced
%                     during the reduction of the original symmetric
%                     matrix to symmetric tridiagonal form
%                     - this input is needed only if computation of the
%                       eigenvectors is requested (by including the second
%                       output argument) and the original matrix was not in
%                       symmetric tridiagonal form
%
%     output:
%             lambda  vector containing the eigenvalues of the symmetric
%                     tridiagonal matrix determined by the vectors a and b
%             v       optional output argument
%                     matrix containing the eigenvectors of the symmetric
%                     tridiagonal matrix determined by the vectors a and b
%                     - the i-th column of this matrix is an eigenvector
%                       which corresponds to the i-th eigenvalue in the
%                       vector lambda
%                     - eigenvectors will be not computed if this second
%                       output argument is omitted
%
%     NOTE:
%             if the maximum number of iterations is exceeded, a message
%             to this effect will be displayed, along with the number of
%             eigenvalues which had been determined - these eigenvalues
%             will be returned in the last entries of the output vector
%             lambda
%

n = length(a);
if ( length(b) == n-1 ) b(2:n) = b(1:n-1); end;

c = zeros ( 1, n );
s = zeros ( 1, n );
shift = 0;
togo = n;

if ( nargout >= 2 )
   if ( nargin >= 5 )
      v = vects;
   else
      v = eye(n);
   end;
end;

for its = 1 : Nmax

    if ( togo == 1 )
	   lambda(1) = a(1) + shift;
	   disp ( its );
	   return;
	end;

    trace = a(togo-1) + a(togo);
	det   = a(togo-1)*a(togo) - b(togo)*b(togo);
	disc  = sqrt ( trace*trace - 4*det );
	mu1 = (1/2) * ( trace + disc );
	mu2 = (1/2) * ( trace - disc );
	if ( abs ( mu1 - a(togo) ) < abs ( mu2 - a(togo) ) )
	   s = mu1;
	else
	   s = mu2;
	end;

    shift = shift + s;
	for i = 1:togo
	    a(i) = a(i) - s;
	end;

	oldb = b(2);
    for i = 2:togo
        j = i-1;
	    r = sqrt ( a(j)^2 + oldb^2 );
	    c(i) = a(j) / r;
	    s(i) = oldb / r;
	    a(j) = r;
	    temp1 = c(i)*b(i) + s(i)*a(i);
	    temp2 = -s(i)*b(i) + c(i)*a(i);
	    b(i) = temp1;
	    a(i) = temp2;
	    if ( i ~= togo ) oldb = b(i+1); b(i+1) = c(i)*b(i+1); end;
    end;

    a(1) = c(2)*a(1) + s(2)*b(2);
    b(2) = s(2)*a(2);
    for i = 2:togo-1
        a(i) = s(i+1)*b(i+1) + c(i)*c(i+1)*a(i);
	    b(i+1) = s(i+1)*a(i+1);
    end;
    a(togo) = c(togo)*a(togo);

	if ( nargout >= 2 )
	   for i = 2 : togo
	       col1 = v(:,i-1) * c(i) + v(:,i) * s(i);
		   v(:,i) = -s(i) * v(:,i-1) + c(i) * v(:,i);
		   v(:,i-1) = col1;
	   end;
	end;

	if ( abs(b(togo)) < TOL )
	   lambda(togo) = a(togo) + shift;
	   disp([lambda(togo) its]);
	   togo = togo - 1;
	end;

end;

disp ( 'qrst error: Maximum number of iterations exceeded' );
disp ( sprintf ( '%d eigenvalues determined \n', n-togo ) );


function [a,b,c,d]=getplanefrom3pt(plane)
%
% [a,b,c,d]=getplanefrom3pt(plane)
%
% define a plane equation ax+by+cz+d=0 from three 3D points
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    plane: a 3x3 matrix with each row specifying a 3D point (x,y,z)
%
% output:
%    a,b,c,d: the coefficient for plane equation ax+by+cz+d=0
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

x=plane(:,1);
y=plane(:,2);
z=plane(:,3);

% compute the plane equation a*x + b*y +c*z +d=0

a=y(1)*(z(2)-z(3))+y(2)*(z(3)-z(1))+y(3)*(z(1)-z(2));
b=z(1)*(x(2)-x(3))+z(2)*(x(3)-x(1))+z(3)*(x(1)-x(2));
c=x(1)*(y(2)-y(3))+x(2)*(y(3)-y(1))+x(3)*(y(1)-y(2));
d=-det(plane);


function euler_m = dcm2euler_m(DCMnb_m)
% dcm2euler_m: transforms a nav-to-body DCM matrix to an Euler angles matrix.
%
% INPUT
%   DCMnb_m: Nx9 matrix with nav-to-body direct cosine matrices (DCM).
% Each row of DCMnb_m contains the 9 elements of a particular DCMnb
% matrix ordered as [a11 a21 a31 a12 a22 a32 a13 a23 a33].
%
% OUTPUT
%   euler_m: Nx3 Euler angles ordered by column, [roll pitch yaw] (rad, rad, rad).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
% 	Titterton, D.H. and Weston, J.L. (2004). Strapdown
% Inertial Navigation Technology (2nd Ed.). Institution
% of Engineering and Technology, USA. Eq. 11.4. Eq. 3.66, p. 46.
%
%   R. Gonzalez, J. Giribet, and H.D. Patiño,. An approach to
% benchmarking of loosely coupled low-cost navigation systems,
% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
% issue 2, pp. 272-287. Eq. 15.
%
% Version: 001
% Date:    2020/12/17
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

[N,~] = size (DCMnb_m);

euler_m = zeros(N,3);

for i=1:N

    DCMnb = reshape(DCMnb_m (i,:), 3, 3);
    DCMbn = DCMnb';                         % nav-to-body > body-to-nav

    euler_m (i,:) = dcm2euler(DCMbn);       % phi theta psi
end

end


function imchi=kkimbook2(omega,rechi,alpha)
%The program inputs are the vector of the frequency (or energy)
%components, the vector of the real part of the susceptibility
%under examination, and the value of the moment considered.
%The two vectors must have the same length
%and the frequency vector omega must be equispaced.
%If not, apply MATLAB functions such as interp.
%If rechi is the real part of a linear susceptibility,
%alpha must be 0.
%If rechi is the real part of the nth
%harmonic generation susceptibility, alpha=0,1,..2n.
%If rechi is the real part of a pump and probe
%susceptibility, alpha=0 or 1.
%This files accompanies the book
%"Kramers-Kronig Relations in Optical Materials Research"
%by Lucarini, V., Saarinen, J.J., Peiponen, K.-E., Vartiainen, E.M.
%Springer, Heidelberg, 2005
%where the theory and applications are fully developed.
%The output is the estimate of the imaginary part as obtained
%with K-K relations.
%This software is distributed under the GNU licence agreement
%by Valerio Lucarini
%email: lucarini@alum.mit.edu
%University of Camerino
%Department of Mathematics and Computer Science
%Camerino, Italy

if size(omega,1)>size(omega,2);
omega=omega';
end; if size(rechi,1)>size(rechi,2);
rechi=rechi';
end;
%Here the program rearranges the two vectors so that,
%whichever their initial shape, they become row vectors.

g=size(omega,2);
%Size of the vectors.%

imchi=zeros(size(rechi));
%The output is initialized.

a=zeros(size(rechi));
b=zeros(size(rechi));
%Two vectors for intermediate calculations are initialized

deltaomega=omega(2)-omega(1);
%Here we compute the frequency (or energy) interval

j=1;
beta1=0;
for k=2:g;
b(1)=beta1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(1)^2);
beta1=b(1);
end;
imchi(1)=-2/pi*deltaomega*b(1)*omega(1)^(1-2*alpha);
%First element of the output: the principal part integration
%is computed by excluding the first element of the input

j=g;
alpha1=0;
for k=1:g-1;
a(g)=alpha1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(g)^2);
alpha1=a(g);
end;
imchi(g)=-2/pi*deltaomega*a(g)*omega(g)^(1-2*alpha);
%Last element of the output: the principal part integration
%is computed by excluding the last element of the input.

for j=2:g-1; ;
%Loop on the inner components of the output vector.
alpha1=0;
beta1=0;
for k=1:j-1;
a(j)=alpha1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(j)^2);
alpha1=a(j);
end;
for k=j+1:g;
b(j)=beta1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(j)^2);
beta1=b(j);
end;
imchi(j)=-2/pi*deltaomega*(a(j)+b(j))*omega(j)^(1-2*alpha);
%Last element of the output: the principal part integration
%is computed by excluding the last element of the input
end;


function [A,Phi] = inverse_cht(Am,Nphi)
%INVERSE_CHT inverse circular harmonics transform (ICHT)
%
%   Usage: [A,Phi] = inverse_cht(Am,[Nphi])
%
%   Input parameters:
%       Am      - circular harmonics coefficients [N x (2*M+1)]
%       Nphi    - number of equi-angular distributed angles, for which the ICHT
%                 is computed, optional, default: 2*M+1
%
%   Output parameters:
%       A       - inverse circular harmonics transform [N x Nphi]
%       Phi     - corresponding angle of the ICHT [1 x Nphi]
%
%   See also: pwd_imp_circexp

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% 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.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 1;
nargmax = 2;
narginchk(nargmin,nargmax);
isargmatrix(Am);
if nargin == nargmin
    Nphi = size(Am, 2);
else
    isargpositivescalar(Nphi);
end

%% ===== Computation ==================================================
M = (size(Am,2)-1)/2;
N = size(Am,1);

% Implementation of
%           ___
%           \
% A(phi) =  /__    A  e^(+i*m*n*2*pi/Nphi)
%         m=-M..M   m

% Spatial IFFT
A = zeros(N, Nphi);
% this handles cases where Nphi < M
for l=1:N
    A(l,:) = sum(buffer(Am(l,:),Nphi),2);
end
A = circshift(A,[0,-M]);  % m = 0, ..., M, ..., -M, ..., -1
A = ifft(A,[],2) * Nphi;  % IFFT includes factor 1/Nphi

% Axis corresponding to ICHT
if nargout>1
    Phi = 0:2*pi / Nphi:2*pi*(1-1/Nphi);
end


function value = lp_value ( n, o, x )

%*****************************************************************************80
%
%% LP_VALUE evaluates a Legendre polynomial at several points X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, integer O, the degree of the polynomial.
%    0 <= O.
%
%    Input, real X(N,1), the evaluation points.
%
%    Output, real VALUE(N,1), the value of the Legendre polynomial
%    of degree O at the points X.
%
  x = x ( : );

  v = zeros ( n, o + 1 );

  v(1:n,1) = 1.0;

  if ( 1 <= o )

    v(1:n,2) = x(1:n,1);

    for j = 2 : o

      v(1:n,j+1) = ( ( 2 * j - 1 ) * x(1:n,1) .* v(1:n,j)   ...
                  -  (     j - 1 ) *             v(1:n,j-1) ) ...
                  /  (     j     );

    end

  end

  value(1:n,1) = v(1:n,o+1);

  return
end


function [r,varargout]=circfit(x,y)
%CIRCFIT  Least squares fit of X-Y data to a circle.
%   R = CIRCFIT(X,Y) returns scalar radius R of a fitted circle. X and Y are 1-D
%   arrays of position data in a rectilinear coordinate system. X and Y must be
%   the same length and must contain at least three non-colinear points in order
%   for a valid solution to be found.
%
%   [R,ERR] = CIRCFIT(X,Y) additionally returns the scalar root mean squared
%   error of the fitted circle radius and center relative to the position data.
%
%   [R,XC,YC] = CIRCFIT(X,Y) additionally returns the scalar positions, XC and
%   YC, of center of the fitted circle.
%
%   [R,XC,YC,ERR] = CIRCFIT(X,Y) returns both the center positions of the circle
%   as well as the root mean squared error.
%
%   Examples:
%       % Fit of just five noisy points
%       x1=[1 0 -1 0 1]+0.05*randn(1,5); y1=[0 1 0 -1 0]+0.05*randn(1,5);
%       r1=circfit(x1,y1)
%
%       % CIRCFIT can sometimes perfom poorly with less than 180-degree arc
%       t=0:0.1:pi; lt=length(t);
%       x2=cos(t)+0.04*randn(1,lt); y2=sin(t)+0.04*randn(1,lt);
%       r2_90deg=circfit(x2(1:floor(lt/2)),y2(1:floor(lt/2)))
%       r2_180deg=circfit(x2,y2)

%   Andrew D. Horchler, adh9 @ case . edu, Created 5-12-7
%   Revision: 1.3, 4-6-16

% Check inputs
if nargout > 4
    error('circfit:circfit:TooManyOutputs','Too many output arguments.');
end

if ~isvector(x) || ~isfloat(x) || ~isreal(x) || ~all(isfinite(x))
    error('circfit:circfit:NonFiniteRealVectorX',...
          'X must be a finite real vector of floating point numbers.');
end
if ~isvector(y) || ~isfloat(y) || ~isreal(y) || ~all(isfinite(y))
    error('circfit:circfit:NonFiniteRealVectory',...
          'Y must be a finite real vector of floating point numbers.');
end

lx=length(x);
if lx ~= length(y)
    error('circfit:circfit:LengthMismatch',...
          'The vectors X and Y must have the same length.');
end
if lx < 3
    error('circfit:circfit:Min3Points',...
          'The vectors X and Y must contain at least three points.');
end
x=x(:);
y=y(:);

% Check collinearity, assume with sufficient points, some will be non-collinear
if rank(diff([x([1:min(50,lx) 1]) y([1:min(50,lx) 1])])) == 1
    if lx <= 50 || rank(diff([x y;x(1) y(1)])) == 1
        error('circfit:circfit:Colinearity',...
             ['The points in vectors X and Y must not all be collinear, or '...
              'nearly collinear, with each other.']);
    end
end

xx=x.*x;
yy=y.*y;
xy=x.*y;
xxyy=xx+yy;
sx=sum(x);
sy=sum(y);
sxx=sum(xx);
syy=sum(yy);
sxy=sum(xy);

% Solve linear system without inverting
% a=[sx sy lx;sxy syy sy;sxx sxy sx]\[sxx+syy;sum(xxyy.*y);sum(xxyy.*x)];
[L,U]=lu([sx sy lx;sxy syy sy;sxx sxy sx]);
a=U\(L\[sxx+syy;sum(xxyy.*y);sum(xxyy.*x)]);

xc=0.5*a(1);          	% X-position of center of fitted circle
yc=0.5*a(2);          	% Y-position of center of fitted circle
r=sqrt(xc^2+yc^2+a(3));	% Radius of fitted circle

% Set variable outputs
if nargout > 2
    varargout{1}=xc;
    varargout{2}=yc;
end
if nargout == 2 || nargout == 4
    varargout{nargout-1}=sqrt(mean((sqrt((x-xc).^2+(y-yc).^2)-r).^2));	% RMSE
end


function [H,F] = make_bank_DFT(p,N)

% make_bank_DFT     Generate DFT Filter Bank with N Subbands
%
% Arguments:
% p                 Prototype filter (impulse response)
% N                 Number of subbands
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

p = p(:);                            % Make column
flen = max(size(p));                 % Length of prototype filter
H = zeros(flen,N/2+1);               % Assume N is an even number
n = (0:flen-1)';

k = 0; H(:,k+1) = p;                 % i = 0
k = N/2; H(:,k+1) = p.*((-1).^n);    % i = N/2+1

for k = 1:(N/2-1)
    H(:,k+1) = p.*exp(j*2*pi/N*k*n); % Complex modulation
end

D = H(:,2:N/2);
H = [H, conj(fliplr(D))];            % Complex-conjugate pair

% Synthesis filters are the same as the analysis filters

F = H;


function f = bohach2_xy ( x, y )

%*****************************************************************************80
%
%% BOHACH2_XY evaluates the Bohachevsky function #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, real X, Y, the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  f =       x * x ...
    + 2.0 * y * y ...
    - 0.3 * cos ( 3.0 * pi * x ) * cos ( 4.0 * pi * y ) ...
    + 0.3;

  return
end


function z = nmi(x, y)
% Compute normalized mutual information I(x,y)/sqrt(H(x)*H(y)) of two discrete variables x and y.
% Input:
%   x, y: two integer vector of the same length
% Ouput:
%   z: normalized mutual information z=I(x,y)/sqrt(H(x)*H(y))
% Written by Mo Chen (sth4nth@gmail.com).
assert(numel(x) == numel(y));
n = numel(x);
x = reshape(x,1,n);
y = reshape(y,1,n);

l = min(min(x),min(y));
x = x-l+1;
y = y-l+1;
k = max(max(x),max(y));

idx = 1:n;
Mx = sparse(idx,x,1,n,k,n);
My = sparse(idx,y,1,n,k,n);
Pxy = nonzeros(Mx'*My/n); %joint distribution of x and y
Hxy = -dot(Pxy,log2(Pxy));

% hacking, to elimative the 0log0 issue
Px = nonzeros(mean(Mx,1));
Py = nonzeros(mean(My,1));

% entropy of Py and Px
Hx = -dot(Px,log2(Px));
Hy = -dot(Py,log2(Py));

% mutual information
MI = Hx + Hy - Hxy;

% normalized mutual information
z = sqrt((MI/Hx)*(MI/Hy));
z = max(0,z);