function sp = spdimcc(n, d)
% SPDIMCC   Compute number of sparse grid points, Clenshaw-Curtis grid
%    SP = SPDIMCC(N,D) Computes the number of sparse grid points
%	   of the Clenshaw-Gurtis grid of dimension D and level N.
%    (Internal function)

% The function uses the formulas for N <= 7 given in A. Schreiber:
% Die Methode von Smoliak bei der multivariaten Interpolation,
% (2000), p.35/36.

% Author : Andreas Klimke, Universitaet Stuttgart
% Version: 1.0
% Date   : August 5, 2003

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

switch n
 case 0
	sp = 1;
 case 1
	sp = 2*d + 1;
 case 2
	sp = 2*d^2 + 2*d + 1;
 case 3
	sp = round((4*d^3 + 6*d^2 + 14*d)/3) + 1;
 case 4
	sp = round((2*d^4 + 4*d^3 + 22*d^2 + 20*d)/3) + 1;
 case 5
	sp = round((4*d^5 + 10*d^4 + 100*d^3 + 170*d^2 + 196*d)/15) + 1;
 case 6
	sp = round((4*d^6 + 12*d^5 + 190*d^4 + 480*d^3 + 1246*d^2 + 948* ...
							d)/45) + 1;
 case 7
	sp = round((8*d^7 + 28*d^6 + 644*d^5 + 2170*d^4 + 9632*d^3 + ...
							15442*d^2 + 12396*d)/315) + 1;
 otherwise
	sp = round((8*d^7 + 28*d^6 + 644*d^5 + 2170*d^4 + 9632*d^3 + ...
							15442*d^2 + 12396*d)/315) + 1;
	for m = 8:n
		seq = spgetseq(m,d);
		for k = 1:size(seq,1)
			temp = 1;
			for l = 1:d
				lval = seq(k,l);
				if lval == 0
				elseif lval < 3
					temp = temp * 2;
				else
					temp = temp * 2^double(lval-1);
				end
			end
			sp = sp + temp;
		end
	end
end


function [DAD,DInv]=equilibrateSymMat(A)
%EQUILIBRATESYMMAT Equilibrate a symmetric matrix maintaining symmetry.
%          This finds a matrix D such that D*A*D is such that the largest
%          magnitude element in each row and in each column is 1. It is
%          assumed that there is no all-zero row in A.
%
%INPUTS: A An nXn symmetric matrix having no all-zero row.
%
%OUTPUTS: DAD The equilibrated matrix. That is A pre and post multiplied by
%             the diagonal matrix D.
%        DInv The inverse of the diagonal matrix D. Invert the diagonal to
%             get D.
%
%This function implements the algorithm of Sectioj 5 of [1].
%
%EXAMPLE 1:
%Here, we create a symmetric matrix and equilibrate it. The equilibration
%makes the largest magnitude values in each row and column equal 1.
%However, it also worsens the condition number, as we shall show:
% V=vander([1;2;3;4]);
% A=V+V';
% DAD=equilibrateSymMat(A);
%
% %The original maximum absolute value elements in the columns and rows.
% max(abs(A),[],1)
% max(abs(A),[],2)
%
% %After equilibration, the maximum mangitude elements in the columns and
% %rows are all 1.
% max(abs(DAD),[],1)
% max(abs(DAD),[],2)
%
% %One can see that the condition number increased.
% cond(A,Inf)
% cond(DAD,Inf)
%
%EXAMPLE 2:
%This is the example given in Section 7 of [1]. AGian, we can see that the
%matrix has been equilibrated, but the condition number got worse.
% A=[1,2;
%    2,1];
% DAD=equilibrateSymMat(A);
%
% %The original maximum absolute value elements in the columns and rows.
% max(abs(A),[],1)
% max(abs(A),[],2)
%
% %After equilibration, the maximum mangitude elements in the columns and
% %rows are all 1.
% max(abs(DAD),[],1)
% max(abs(DAD),[],2)
%
% %One can see that the condition number increased.
% cond(A,Inf)%The inital condition number is 3.
% cond(DAD,Inf)%The equilibrated condition number is 16/3.
%
%REFERENCES:
%[1] J. R. Bunch, "Equilibration of symmetric matrices in the max-norm,"
%    Journal of the Association for Computing Machinery, vol. 18, no. 4,
%    pp. 566-572, Oct. 1971.
%
%September 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);

d=zeros(n,1);
for i=1:n
    d(i)=sqrt(abs(A(i,i)));
    for j=1:(i-1)
        t=abs(A(i,j));
        d(i)=max(t,d(i));
    end
    if(d(i)~=0)
        for j=1:i
            A(i,j)=A(i,j)/d(i);
        end

        for j=i:n
            A(j,i)=A(j,i)/d(i);
        end
    end
end

for i=1:n
    if(d(i)==0)
        for j=(i+1):n
            t=abs(A(j,i));
            d(i)=max(t,d(i));
        end

        if(d(i)==0)
            error('The matrix has an all-zero row.')
        end
        for j=(i+1):n
            A(j,i)=A(j,i)/d(i);
        end
    end
end

L=tril(A);
DAD=L+L'-diag(diag(L));

DInv=diag(d);

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: computes the angular displacements of a damped pendulum under
%           gravity
%
% PURPOSE: to illustrate that changing the damping parameter, b,
%          significantly affects the resulting dynamics of the pendulum system
%
% Author: N.A. Battista
% Date: 12/13/2019
% Institution: The College of New Jersey
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function damped_pendulum_diff_b()

%
% Physical Parameters
%
m = 100;    % Mass of Pendulum bob
g=9.80665;  % Gravitational Acceleration;
L=0.2;      % Length of Pendulum

%
% Radii of Bobs (here - all the same)
%
rVec = 100*0.005*ones(1,6);

%
% Damping Parameter (here - each case is different)
%
bVec = 100*[0 0.1 0.2 0.4 1.0 2.0];

%
% Temporal Parameters
%
t=0;        % initial time
tFinal = 8; % final-time
dt = 1e-3;  % time-step
ct=1;       % counter
tVec(ct)=t; % time vector storage

%
% Moment of Inertia
%
IVec = m*(0.5*rVec.^2+L^2);

%
% Initial Angular Displacement
%
ang = -(pi/2-pi/5)*ones(1,6);
phi = zeros( size( ang ) );

%
% Perform the time-stepping, e.g., solve the ODEs
%
while t<=tFinal

    % increment the storage counter
    ct = ct+1;

    % increment time
    t = t+dt;

    % save current time
    tVec(ct) = t;

    % solve 1st order ODE for angular displacement
    ang(ct,:) = ang(ct-1,:) + dt * ( phi(ct-1,:) );

    % solve 1st order ODE for angular velocity
    phi(ct,:) = phi(ct-1,:) + dt * ( -m*g*L ./IVec .* sin( ang(ct-1,:) ) - bVec.*phi(ct-1,:)./IVec );

end

%
% Defining Colors for Plotting Data
%
color2 = [0.635 0.078 0.184];  % dark red
color5 = [0.9 0.525 0.098];    % orange
color6 = [0.929 0.840 0.1250]; % dark yellow
color7 = [0.7 0.7 0];          % 'ugly' green
color13 = [0 0.5 0.5];             % green

%
% Note discrepancy is a bit off due to small angle approximation:
% sin(theta) ~ theta gives below period of oscillation
%
T1 = 2*pi*sqrt( IVec(1) / (m*g*L) );
%
% Actually computed value of period is:
T1 = 1.93;

%
% Make Figure of Angular Displacement vs. Non-dimensional Time (# of oscillations of 'r' case)
%
lw=6;
fs=18;
plot(tVec/T1,ang(:,1),'k-','LineWidth',lw); hold on;
plot(tVec/T1,ang(:,2),'b-','LineWidth',lw,'Color',color2); hold on;
plot(tVec/T1,ang(:,3),'g-','LineWidth',lw,'Color',color5); hold on;
plot(tVec/T1,ang(:,4),'r-','LineWidth',lw,'Color',color6); hold on;
plot(tVec/T1,ang(:,5),'c-','LineWidth',lw,'Color',color7); hold on;
plot(tVec/T1,ang(:,6),'c-','LineWidth',lw,'Color',color13); hold on;
xlabel('Non-Dimensional Time (# periods of r-case)');
ylabel('Angular Displacement (Radians)');
axis([0 4.1 -1 1.5]);
leg=legend('b=0','b=10','b=20','b=40','b=100','b=200','Orientation','horizontal');
set(gca,'FontSize',fs);
set(leg,'FontSize',fs);
leg.NumColumns=3;


function PA=partialMatrixInverse(A,n)
%%PARTIALMATRIXINVERSE Given an mXm possibly singular matrix A of a rank
%   >=n, where the first n columns are independent, obtain a matrix that is
%   akin to the inverse related to the first n columns. A normal matrix
%   inverse is such that A*inv(A)=eye(m,m). Here, consider A=Q*R, where Q
%   is an orthogonal matrix and R is a lower-triangular matrix (not the
%   standard QR decomposition). The inverse relation is Q*R*inv(A)=eye(m,m)
%   or R*inv(A)=Q'*eye(m,m). This function solves for the nXn upper-left
%   submatrix of inv(A).  This function is useful for taking a singular
%   Fisher informationmatrix, where the first n rows are observable, but
%   the rest are not, and obtaining a Cramer-Rao lower bound for the
%   observable components. Typically, this might be obtaining a CRLB on
%   position components before velocity is fully observable.
%
%INPUTS: A An mXm matrix.
%        n A number n<=m that is <= the rank of A.
%
%OUTPUTS: PA The partial inverse matrix as described above.
%
%This function solves the problem by calling linEqSolveFirstnRows with A
%and vectors [1;0;0;0;...], [0;1;0;0;...], etc. The solution coincides with
%a submatrix of the pseudoinverse.
%
%EXAMPLE:
%This example of the CRLB of a position estimate of a target given a single
%range, direction cosine and range-rate measurement is considered. We show
%that when this function is used, the position accuracy implied by the CRLB
%is not affected by the accuracy of the range rate component (which one
%would assume). However, using the ad-hoc method of taking the inverse of a
%subset of the Fisher information matrix, one gets a different (wrong)
%answer the more accurate the range rate component is. That is due to not
%properly accounting for matrix cross terms.
% %Sensor location near Maui.
% llhRxTx=[deg2rad([20.888645;-156.022474]);0];
% %Convert to Cartesian.
% xRxTx=zeros(6,1);
% %Fill in the position. The reciever will be stationary (0 velocities).
% %Take the first one to be the transmitter.
% xRxTx(1:3,:)=ellips2Cart(llhRxTx);
%
% %Sensor orientation.
% elAboveLevel=deg2rad(15);%The radar point slightly up.
% %Rotation matrices of the radar.
% MRxTx=findRFTransParam(llhRxTx(:,1),pi/2,elAboveLevel);%Facing East
%
% %Construct a target state vector.
% llhTar=[deg2rad([21.14146;-155.058839]);13e3];
% tarLocCart=ellips2Cart(llhTar);
% tarHeading=deg2rad(-140);%Radians East of North.
% angUpFromLevel=0;%Level flight.
% tarSpeed=300;%m/s
% vECEFTar=geogHeading2uVec(llhTar,tarHeading,angUpFromLevel)*tarSpeed;
% xTar=[tarLocCart;vECEFTar];
%
% %Measuremnet and noise parameters of the radar.
% useHalfRange=false;
% sigmaR=10;%Range standard deviation.
% sigmaU=0.01;
% sigmaV=0.01;
% sigmaRR=1;%Range rate standard deviation, meters/ second.
% sigmaRRMassive=1e5;%Effectively uninformative standard deviation.
% %Lower-triangular square root measurement covariance matrix (No cross
% %terms).
% SR=diag([sigmaR;sigmaU;sigmaV;sigmaRR]);
% RInv=inv(SR*SR');
% %The same thing but with the range rate standard deviation so massive it is
% %essentially uninformative.
% SRMassive=diag([sigmaR;sigmaU;sigmaV;sigmaRRMassive]);
% RInvMassive=inv(SRMassive*SRMassive');
%
% %Gradients for the Fisher information matrices.
% H=calcRuvRRJacob(xTar,useHalfRange,xRxTx(:,1),xRxTx(:,1),MRxTx(:,:,1));
% FIM=observedFisherInfo([],RInv,[],H);
% %The position RMSE from the CRLB found correctly using this function.
% RMSE=sqrt(sum(diag(partialMatrixInverse(FIM,3))))
% %An ad-hoc (wrong) approach that one might be tempted to take:
% RMSEWrong=sqrt(sum(diag(inv(FIM(1:3,1:3)))))
% %One can see that the position RMSE is higher when done the correct way.
% %However, a single range rate on its own doesn't inform on the position
% %accuracy. If we do the same thing cranking up the standard deviation of
% %the range rate so it is uninformative, we get:
% FIM=observedFisherInfo([],RInvMassive,[],H);
% RMSEUninfRR=sqrt(sum(diag(partialMatrixInverse(FIM,3))))
% RMSEUninfRRWrong=sqrt(sum(diag(inv(FIM(1:3,1:3)))))
% %In other words, the correct solution doesn't change, but in this case the
% %wrong solution has become correct, because the cross terms that messed it
% %up are essentially gone.
%
%February 2022 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

m=size(A,1);
PA=zeros(n,n);

if(n>m)
    error('n cannot be > the length of A.')
end

e=zeros(m,1);
for k=1:n
    e(k)=1;
    PA(:,k)=linEqSolveFirstnRows(A,e,n);
    e(k)=0;
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 [ axis, angle ] = rotation_mat2axis_3d ( a )

%*****************************************************************************80
%
%% ROTATION_MAT2AXIS_3D converts a rotation from matrix to axis format in 3D.
%
%  Discussion:
%
%    The computation is based on the fact that a rotation matrix must
%    have an eigenvector corresponding to the eigenvalue of 1, hence:
%
%      ( A - I ) * v = 0.
%
%    The eigenvector V is the axis of rotation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Kuipers,
%    Quaternions and Rotation Sequences,
%    Princeton, 1998.
%
%  Parameters:
%
%    Input, real A(3,3), the rotation matrix.
%
%    Output, real AXIS(3), the axis vector which remains
%    unchanged by the rotation.
%
%    Output, real ANGLE, the angular measurement of the
%    rotation about the axis, in radians.
%
  dim_num = 3;
%
%  Compute the normalized axis of rotation.
%
  axis(1) = a(3,2) - a(2,3);
  axis(2) = a(1,3) - a(3,1);
  axis(3) = a(2,1) - a(1,2);

  axis_norm = sqrt ( sum ( axis(1:dim_num).^2 ) );

  if ( axis_norm == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ROTATION_MAT2AXIS_3D - Fatal error!\n' );
    fprintf ( 1, '  A is not a rotation matrix,\n' );
    fprintf ( 1, '  or there are multiple axes of rotation.\n' );
    error ( 'ROTATION_MAT2AXIS_3D - Fatal error!' );
  end

  axis(1:dim_num) = axis(1:dim_num) / axis_norm;
%
%  Find the angle.
%
  angle = r8_acos ( 0.5 * ( a(1,1) + a(2,2) + a(3,3) - 1.0 ) );

  return
end


% 梯形公式的加速--外推技巧
clear; clc;
f = @(x) sqrt(x.^3);
a = 0;
b = 1;
tol = 1e-7;
maxit = 100; % 最大迭代步数

T(1,1) = (b-a)/2 * (f(a) + f(b));
fprintf(' T(1,1)=%.8f \n',T(1,1));
k = 1;  err = 1.0;
for k = 1 : maxit
    h = (b-a)/(2^(k-1));
    T(k+1,1) = T(k,1)/2 + (h/2) * sum(f(a+0.5*h+[0:2^(k-1)-1]*h));
    fprintf(' T(%d,1)=%.8f ',k+1,T(k+1,1));
    for j = 1 : k
        T(k+1,j+1) = (4^j * T(k+1,j) - T(k,j) )/(4^j-1);
        fprintf(' T(%d,%d)=%.8f ',k+1,j+1,T(k+1,j+1));
        err = abs(T(k+1,j+1)-T(k+1,j));
        if (err<tol), break, end % check convergence
    end
    fprintf('\n')
    if (err<tol), break, end % check convergence
end


function p = protate ( p, phi )

%*****************************************************************************80
%
%% PROTATE rotates a set of points by a given angle.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input/output, real P(NP,2), a set of points to be rotated.
%
%    Input, real PHI, the rotation angle, in radians.
%

%
%  Compute the rotation matrix.
%
  A = [ cos(phi), -sin(phi); ...
        sin(phi),  cos(phi) ];
%
%  Multiply the points by the rotation matrix.
%
  p = p * A;

  return
end


function f = powell ( m, x )

%*****************************************************************************80
%
%% POWELL computes the Powell singular quartic function.
%
%  Discussion:
%
%    This function has a global minimizer:
%
%      X* = ( 0.0, 0.0, 0.0, 0.0 ), F(X*) = 0.
%
%    Start the search at
%
%      X = ( 3.0, -1.0, 0.0, 1.0 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Michael Powell,
%    An Iterative Method for Finding Stationary Values of a Function
%    of Several Variables,
%    Computer Journal,
%    Volume 5, 1962, pages 147-151.
%
%  Parameters:
%
%    Input, integer M, the number of variables.
%
%    Input, real X(M), the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  f1 = x(1) + 10.0 * x(2);
  f2 =                            x(3) - x(4);
  f3 =               x(2) - 2.0 * x(3);
  f4 = x(1)                            - x(4);

  f = f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4;

  return
end


function [ value, ifault ] = digamma ( x )

%*****************************************************************************80
%
%% DIGAMMA calculates DIGAMMA ( X ) = d ( LOG ( GAMMA ( X ) ) ) / dX
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 June 2013
%
%  Author:
%
%    Original FORTRAN77 version by Jose Bernardo.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jose Bernardo,
%    Algorithm AS 103:
%    Psi ( Digamma ) Function,
%    Applied Statistics,
%    Volume 25, Number 3, 1976, pages 315-317.
%
%  Parameters:
%
%    Input, real X, the argument of the digamma function.
%    0 < X.
%
%    Output, real DIGAMMA, the value of the digamma function at X.
%
%    Output, integer IFAULT, error flag.
%    0, no error.
%    1, X <= 0.
%

%
%  Check the input.
%
  if ( x <= 0.0 )
    value = 0.0;
    ifault = 1;
    return
  end
%
%  Initialize.
%
  ifault = 0;
  value = 0.0;
%
%  Use approximation for small argument.
%
  if ( x <= 0.00001 )
    euler_mascheroni = 0.57721566490153286060;
    value = - euler_mascheroni - 1.0 / x;
    return
  end
%
%  Reduce to DIGAMA(X + N).
%
  while ( x < 8.5 )
    value = value - 1.0 / x;
    x = x + 1.0;
  end
%
%  Use Stirling's (actually de Moivre's) expansion.
%
  r = 1.0 / x;
  value = value + log ( x ) - 0.5 * r;
  r = r * r;
  value = value ...
    - r * ( 1.0 / 12.0 ...
    - r * ( 1.0 / 120.0 ...
    - r *   1.0 / 252.0 ) );

  return
end


function [mu ul ll] = circ_mean(alpha, w, dim)
%
% mu = circ_mean(alpha, w)
%   Computes the mean direction for circular data.
%
%   Input:
%     alpha	sample of angles in radians
%     [w		weightings in case of binned angle data]
%     [dim  compute along this dimension, default is 1]
%
%     If dim argument is specified, all other optional arguments can be
%     left empty: circ_mean(alpha, [], dim)
%
%   Output:
%     mu		mean direction
%     ul    upper 95% confidence limit
%     ll    lower 95% confidence limit
%
% PHB 7/6/2008
%
% References:
%   Statistical analysis of circular data, N. I. Fisher
%   Topics in circular statistics, S. R. Jammalamadaka et al.
%   Biostatistical Analysis, J. H. Zar
%
% Circular Statistics Toolbox for Matlab

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

if nargin < 3
  dim = 1;
end

if nargin < 2 || isempty(w)
  % if no specific weighting has been specified
  % assume no binning has taken place
	w = ones(size(alpha));
else
  if size(w,2) ~= size(alpha,2) || size(w,1) ~= size(alpha,1)
    error('Input dimensions do not match');
  end
end

% compute weighted sum of cos and sin of angles
r = nansum(w.*exp(1i*alpha),dim);

% obtain mean by
mu = angle(r);

% confidence limits if desired
if nargout > 1
  t = circ_confmean(alpha,0.05,w,[],dim);
  ul = mu + t;
  ll = mu - t;
end


function result = p00_rat_transform ( problem, order )

%*****************************************************************************80
%
%% P00_RAT_TRANSFORM applies a rational transform and Gauss-Legendre rule.
%
%  Discussion:
%
%    To approximate:
%
%      Integral ( alpha <= x < Infinity ) f(x) dx
%
%    Transform:
%
%      u = 1 / ( 1 + x )
%      du = - dx / ( 1 + x )^2
%
%      x = ( 1 - u ) / u
%      dx = - du / u^2
%
%      x = alpha    => u = 1 / ( 1 + alpha )
%      x = Infinity => u = 0
%
%    Transformed integral:
%
%      Integral ( 0 < u <= 1 / ( 1 + alpha ) ) f ( ( 1 - u ) / u ) du / u^2
%
%    We apply a Gauss-Legendre rule here, but we could easily use any rule
%    that avoids evaluation at U = 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer PROBLEM, the index of the problem.
%
%    Input, integer ORDER, the order of the Gauss-Legendre rule
%    to apply.
%
%    Output, real RESULT, the approximate integral.
%
  alpha = p00_alpha ( problem );
%
%  Get the abscissas and weights for Gauss-Legendre quadrature.
%
  [ u, weight ] = legendre_compute ( order );
%
%  Modify the weights from [-1,1] to [0,1/(1+alpha)].
%
  weight(1:order) = weight(1:order) / 2.0 / ( 1.0 + alpha );
%
%  Linear transform of abscissas from [-1,1] to [0,1/(1+alpha)].
%
  u(1:order) = ( ( 1.0 + u(1:order) ) / ( 1.0 + alpha ) ...
               + ( 1.0 - u(1:order) ) * 0.0 )           ...
               / ( 2.0              );
%
%  Define U_RAT = ( 1 - U ) / U.
%
  u_rat(1:order) = ( 1.0 - u(1:order) ) ./ u(1:order);
%
%  Evaluate F ( (1-U)/U ).
%
  f_vec = p00_fun ( problem, order, u_rat );
%
%  The integrand is F ( (1-U)/U ) / U^2
%
  f_vec(1:order) = f_vec(1:order) ./ u(1:order).^2;
%
%  Sum.
%
  result = weight(1:order) * f_vec(1:order)';

  return
end


function [ an, bn ] = lcrg_anbn ( a, b, c, n )

%*****************************************************************************80
%
%% LCRG_ANBN computes the "N-th power" of a linear congruential generator.
%
%  Discussion:
%
%    We are considering a linear congruential random number generator.
%    The LCRG takes as input an integer value called SEED, and returns
%    an updated value of SEED,
%
%      SEED(out) = ( a * SEED(in) + b ) mod c.
%
%    and an associated pseudorandom real value
%
%      U = SEED(out) / c.
%
%    In most cases, a user is content to call the LCRG repeatedly, with
%    the updating of SEED being taken care of automatically.
%
%    The purpose of this routine is to determine the values of AN and BN
%    that describe the LCRG that is equivalent to N applications of the
%    original LCRG.
%
%    One use for such a facility would be to do random number computations
%    in parallel.  If each of N processors is to compute many random values,
%    you can guarantee that they work with distinct random values
%    by starting with a single value of SEED, using the original LCRG to generate
%    the first N-1 "iterates" of SEED, so that you now have N "seed" values,
%    and from now on, applying the N-th power of the LCRG to the seeds.
%
%    If the K-th processor starts from the K-th seed, it will essentially
%    be computing every N-th entry of the original random number sequence,
%    offset by K.  Thus the individual processors will be using a random
%    number stream as good as the original one, and without repeating, and
%    without having to communicate.
%
%    To evaluate the N-th value of SEED directly, we start by ignoring
%    the modular arithmetic, and working out the sequence of calculations
%    as follows:
%
%      SEED(0)   =     SEED.
%      SEED(1)   = a * SEED      + b
%      SEED(2)   = a * SEED(1)   + b = a^2 * SEED           + a * b + b
%      SEED(3)   = a * SEED(2)   + b = a^3 * SEED + a^2 * b + a * b + b
%      ...
%      SEED(N-1) = a * SEED(N-2) + b
%
%      SEED(N) = a * SEED(N-1) + b = a^N * SEED
%                                    + ( a^(n-1) + a^(n-2) + ... + a + 1 ) * b
%
%    or, using the geometric series,
%
%      SEED(N) = a^N * SEED + ( a^N - 1) / ( a - 1 ) * b
%              = AN * SEED + BN
%
%    Thus, from any SEED, we can determine the result of N applications of the
%    original LCRG directly if we can solve
%
%      ( a - 1 ) * BN = ( a^N - 1 ) * b in modular arithmetic,
%
%    and evaluate:
%
%      AN = a^N
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Barry Wilkinson, Michael Allen,
%    Parallel Programming:
%    Techniques and Applications Using Networked Workstations and Parallel Computers,
%    Prentice Hall,
%    ISBN: 0-13-140563-2,
%    LC: QA76.642.W54.
%
%  Parameters:
%
%    Input, integer A, the multiplier for the LCRG.
%
%    Input, integer B, the added value for the LCRG.
%
%    Input, integer C, the base for the modular arithmetic.
%    For 32 bit arithmetic, this is often 2^31 - 1, or 2147483647.  It is
%    required that 0 < C.
%
%    Input, integer N, the "index", or number of times that the
%    LCRG is to be applied.  It is required that 0 <= N.
%
%    Output, integer AN, BN, the multiplier and added value for
%    the LCRG that represent N applications of the original LCRG.
%
  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LCRG_ANBN - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of N = %d\n', n );
    error ( 'LCRG_ANBN - Fatal error!' );
  end

  if ( c <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LCRG_ANBN - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of C = %d\n', c );
    error ( 'LCRG_ANBN - Fatal error!' );
  end

  if ( n == 0 )
    an = 1;
    bn = 0;
  elseif ( n == 1 )
    an = a;
    bn = b;
  else
%
%  Compute A^N.
%
    an = power_mod ( a, n, c );
%
%  Solve
%    ( a - 1 ) * BN = ( a^N - 1 ) mod B
%  for BN.
%
    am1 = a - 1;
    anm1tb = ( an - 1 ) * b;

    [ bn, ierror ] = congruence ( am1, c, anm1tb );

    if ( ierror ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'LCRG_ANBN - Fatal error!\n' );
      fprintf ( 1, '  An error occurred in the CONGRUENCE routine.\n' );
      fprintf ( 1, '  The error code was IERROR = %d\n', ierror );
      error ( 'LCRG_ANGN - Fatal error!' );
    end

  end

  return
end


function a = identity ( m, n )

%*****************************************************************************80
%
%% IDENTITY returns the IDENTITY matrix.
%
%  Formula:
%
%    if ( I = J )
%      A(I,J) = 1
%    else
%      A(I,J) = 0
%
%  Example:
%
%    M = 4, N = 5
%
%    1 0 0 0 0
%    0 1 0 0 0
%    0 0 1 0 0
%    0 0 0 1 0
%
%  Rectangular properties:
%
%    A is integral: int ( A ) = A.
%
%    A is a zero/one matrix.
%
%  Square Properties:
%
%    A is nonsingular.
%
%    A is involutional: A * A = I.
%
%    A is diagonal.
%
%    Because A is diagonal, it has property A.
%
%    A is symmetric: A' = A.
%
%    A is a circulant matrix: each row is shifted once to get the next row.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    LAMBDA(1:N) = 1
%
%    The matrix of eigenvectors of A is A.
%
%    det ( A ) = 1.
%
%    A is unimodular.
%
%    For any vector v, A*v = v.
%
%    For any matrix B, A*B = B*A=B.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A is centrosymmetric: A(I,J) = A(N+1-I,N+1-J).
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of
%    the matrix.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n
      if ( i == j )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end
    end
  end

  return
end


function value = p08_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P08_F evaluates the integrand for problem 08.
%
%  Dimension:
%
%    DIM_NUM arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integrand:
%
%    ( sin ( (pi/4) * sum ( x(1:dim_num) ) ) )**2
%
%  Exact Integral:
%
%    1/2 - sqrt ( 2**(3*DIM_NUM) ) * cos ( DIM_NUM * pi / 4 ) ) / ( 2 * pi**DIM_NUM )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Crandall,
%    Projects in Scientific Computing,
%    Springer, 2000,
%    ISBN: 0387950095,
%    LC: Q183.9.C733.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = ( sin ( pi * sum ( x(1:dim_num,point) ) / 4.0 ) )^2;
  end

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

  return
end


function [ out ] = proj_Euclidean_ball( x,c,r )
%PROJ_EUCLIDEAN_BALL computes the orthogonal projection onto the Euclidean ball {x:||x-c||<=r}
%
%  Usage:
%  out = PROJ_EUCLIDEAN_BALL(x,[c],[r])
%  ===========================================
%  Input:
%  x - point to be projected (vector/matrix)
%  c - center of the ball (vector/matrix) [default: c=0]
%  r - positive radius (a positive scalar) [default: r=1]
%  ===========================================
%  Assumptions:
%  l2 or Frobenius norm
%  ===========================================
%  Output:
%  out - projection vector

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% 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/>.

%reading the user x and setting defalut values when required.
if (nargin < 1)
     error ('usage: proj_Euclidean_ball(input,[c],[r])') ;
end

if (nargin < 3)
    %setting default value of r to 1
    r = 1;
end

if ((nargin < 2) || (isempty (c)))
    %setting default value of c to zeros
    c  = zeros(size(x)) ;
end

out=  c+r *(x-c) / max(norm(x-c,'fro'),r) ;
end


% Simple algorithm to calculate the deflection, bending moment, shear force in a
% cantilever beam.
% load = loading distribution

% Example,
% 1.Uniform loading of 10 N/m: load=repmat(10,1,1001), l=20, dl=0.02
% 2.Point loading with P=400 N, load=[repmat(0,1,999) 10000 10000], l=20, dl=0.02

% length(load)=(l/dl)

function deflection=beam_deflection(load,ei,l,dl)
if length(load)~=(l/dl+1)
    error('Check inputs')
end
y=0:dl:l;
m=sum((y.*load))*dl;
v=sum(load)*dl;
u_4=load/ei;
u_3=v/ei;
for i=2:length(load)
    u_3(i)=u_3(i-1)-u_4(i-1)*dl;
end
u_2=m/ei;
for i=2:length(load)
    u_2(i)=u_2(i-1)-u_3(i-1)*dl;
end
u_1=0;
for i=2:length(load)
    u_1(i)=u_1(i-1)+u_2(i-1)*dl;
end
u=0;
for i=2:length(load)
    u(i)=u(i-1)+u_1(i-1)*dl;
end
deflection=u;
plot(y,u_2*ei)
hold on
plot(y,u_3*ei,'r')
legend('bending moment','shear force')
xlabel('length along the beam')
ylabel(' bending moment and shear force (SI units)')
grid
hold off
figure,plot(y,u,'r')
xlabel('length along the beam')
ylabel('deflection')
grid
title('Deflection')


function [II,DA] = inv_3x3(AA)
%INV_3X3 calc. the inverses for a block of 3-by-3 matrices.
%   [IA,DA] = INV_3X3(AA) returns a set of 'inverses' IA and
%   an array of determinants DA for the set of 3-by-3 linear
%   systems in AA. SIZE(AA), SIZE(IA) = [3,3,N], where N is
%   the number of linear systems. DA is an N-by-1 array of
%   determinant values. Note that each IA(:,:,K) is an 'inc-
%   omplete inverse DET(A(:,:,K)) * A(:,:,K)^(-1) to improve
%   numerical robustness. To solve a linear system, A*X = B,
%   compute (I*B)./D, given D is non-zero.
%
%   See also INV_2X2

%   Darren Engwirda : 2018 --
%   Email           : de2363@columbia.edu
%   Last updated    : 13/02/2020

%---------------------------------------------- basic checks
    if (  ~isnumeric(AA))
        error('inv_3x3:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(AA) >  +3 )
        error('inv_3x3:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(AA,1)~= +3 || ...
        size(AA,2)~= +3 )
        error('inv_3x3:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%---------------------------------------------- build inv(A)
    II = zeros(size (AA)) ;

    DA = det_3x3(AA) ;

    II(1,1,:) = ...
    AA(3,3,:) .* AA(2,2,:) ...
  - AA(3,2,:) .* AA(2,3,:) ;

    II(1,2,:) = ...
    AA(3,2,:) .* AA(1,3,:) ...
  - AA(3,3,:) .* AA(1,2,:) ;

    II(1,3,:) = ...
    AA(2,3,:) .* AA(1,2,:) ...
  - AA(2,2,:) .* AA(1,3,:) ;

    II(2,1,:) = ...
    AA(3,1,:) .* AA(2,3,:) ...
  - AA(3,3,:) .* AA(2,1,:) ;

    II(2,2,:) = ...
    AA(3,3,:) .* AA(1,1,:) ...
  - AA(3,1,:) .* AA(1,3,:) ;

    II(2,3,:) = ...
    AA(2,1,:) .* AA(1,3,:) ...
  - AA(2,3,:) .* AA(1,1,:) ;

    II(3,1,:) = ...
    AA(3,2,:) .* AA(2,1,:) ...
  - AA(3,1,:) .* AA(2,2,:) ;

    II(3,2,:) = ...
    AA(3,1,:) .* AA(1,2,:) ...
  - AA(3,2,:) .* AA(1,1,:) ;

    II(3,3,:) = ...
    AA(2,2,:) .* AA(1,1,:) ...
  - AA(2,1,:) .* AA(1,2,:) ;

end

function [DA]    = det_3x3(AA)

    DA = ...
    AA(1,1,:) .* ( ...
	AA(2,2,:) .* AA(3,3,:) ...
  - AA(2,3,:) .* AA(3,2,:) ...
	    ) - ...
    AA(1,2,:) .* ( ...
	AA(2,1,:) .* AA(3,3,:) ...
  - AA(2,3,:) .* AA(3,1,:) ...
	    ) + ...
	AA(1,3,:) .* ( ...
	AA(2,1,:) .* AA(3,2,:) ...
  - AA(2,2,:) .* AA(3,1,:) ...
	    ) ;

end