function U=NTds(U,X,Y,xflux,yflux,N,T,varargin)
%%
%   Performs dimensional splitting on the 2D scalar conservation law
%   U_t + xflux(U)_x + yflux(U)_y = 0 with initial data U.
%   Uses Strang splitting with timestep dt=T/N. The domain is the square
%   spanned by X and Y. varargin may contain 'wbar' if waitbar is desired,
%   'periodic' followd by [xmin xmax], [ymin ymax]. The one-dimensional
%   equations are solved using a second order central scheme.
%

%%
% Initial setup
addpath ../../AppendixA;
wb=0;
nv=length(varargin);
i=1;
boundary_cond='neumann';
while i<=nv,
	if strcmp(varargin(i),'wbar'),
		wb=1;
		i=i+1;
	elseif strcmp(varargin(i),'periodic'),
		boundary_cond='periodic';
		i=i+1;
	else
      display('Unknown option in NTds');
      i=i+1;
	end;
end;
dt=T/N;
xdir=2; ydir=1;
dx=X(2)-X(1);
dy=Y(2)-Y(1);
CFL=0.5;
limiter='superbee';
strang=dt*0.5;

%%
% The main loop
if wb
  wstr=sprintf('Computing %d steps.',N);
  wbar=waitbar(0,wstr);
end;
for k=1:N,
  U=central(xflux,U,strang,dx,xdir,boundary_cond,CFL,limiter);
  if wb,
    waitbar((k-0.5)/N,wbar);
  end;
  if (k==1),
    strang=dt;
  end;
  U=central(yflux,U,strang,dy,ydir,boundary_cond,CFL,limiter);
  if k==N,
    strang=0.5*dt;
    U=central(xflux,U,strang,dx,xdir,boundary_cond,CFL,limiter);
  end;
  if wb,
    waitbar(k/N,wbar);
  end;
end;

%%
%
if wb,
  close(wbar)
end;
rmpath ../../AppendixA;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   example 2 DOF planar robot.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   03/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

robot.name='Example 2DOF planar arm';

%kinematic data DH parameters
robot.DH.theta='[q(1) q(2)]';
robot.DH.d='[0  0]';
robot.DH.a='[1  1]';
robot.DH.alpha='[0  0]';

%number of degrees of freedom
robot.DOF = 2;

%rotational: R, translational: T
robot.kind=['R' 'R'];

%Jacobian matrix. It is easy to obtain the Jacobian matrix of this robot
% J is defined such that:
% v=[vn wn]', where vn is the linear speed of the end effector and wn in
% the angular speed. vn=[vx vy vz] and wn = [wx wy wz]. Being this a planar
% robot, vz=0 and wx=wy=0
robot.J=['[-a(1)*sin(q(1))-a(2)*sin(q(1)+q(2))  -a(2)*sin(q(1)+q(2));' ...
          'a(1)*cos(q(1))+a(2)*cos(q(1)+q(2))   a(2)*cos(q(1)+q(2));' ...
          '               0                                  0;' ...
          '               0                                  0;' ...
          '               0                                  0;' ...
          '               1                                  1]'];
%Function name to compute inverse kinematic
robot.inversekinematic_fn = 'inversekinematic_2dofplanar(robot, T)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(100);
                deg2rad(100)];

robot.accelmax=robot.velmax/3; % 0.1 is here an acceleration time

% end effectors maximum velocity
robot.linear_velmax = 0.5; %m/s,

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GRAPHICS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%read graphics files
robot.graphical.has_graphics=1;
robot.graphical.color = [25 20 40];
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis = [-2.2 2.2 -2.2 2.2 0 2.2];
robot = read_graphics(robot);

% INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
% position, velocity and acceleration
robot.q=[0 0]';
robot.qd=[0 0]';
robot.qdd=[0 0]';
robot.time = [];

robot.q_vector=[];
robot.qd_vector=[];
robot.qdd_vector=[];

robot.last_target=directkinematic(robot, robot.q);
robot.last_zone_data = 'fine';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction or not
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[2 2];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.5      0         0; %(rx, ry, rz) link 1, w/r to reference system 1
                      -0.5      0         0];%(rx, ry, rz) link 2

%link masses
m1 = robot.dynamics.masses(1);
m2 = robot.dynamics.masses(2);

%eval a to obtain parameters.
a=eval(robot.DH.a);
L1 = a(1);
L2 = a(2);

%Inertia matrices of each link
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, or each row
robot.dynamics.Inertia=[0   m1*L1^2/12   m1*L1^2/12    0	0	0;
                        0   m2*L2^2/12   m2*L2^2/12    0	0	0];

%Inertia of the rotor
robot.motors.Inertia=[0 0];
%Reduction ratio: motor_speed/joint speed
robot.motors.G=[10  10];

%Viscous friction factor of the motor
robot.motors.Viscous = [0.9  0.9];
%Coulomb friction of the motor
%Tc+, Tc-
robot.motors.Coulomb = [0	0;
                        0	0];

%Obtained from motor catalog under practicals/inverse_dynamics
%                        R(Ohm)  L(H)      Kv (V/rad/s):speed constant     Kp (Nm/A):torque constant        Max_current (A)
robot.motors.constants=[0.345  0.273e-3       2.3474e-05               84.9e-3                 139];%these correspond to Maxon, 167132;


function x = blend_rs_1dn ( r, s, n, bound_rs )

%*****************************************************************************80
%
%% BLEND_RS_1DN extends vector data along sides into a square.
%
%  Diagram:
%
%    01-----r1-----11
%     |      .      |
%     |      .      |
%    0s.....rs.....1s
%     |      .      |
%     |      .      |
%    00-----r0-----10
%
%  Discussion:
%
%    BLEND_RS_1DN is NOT equivalent to a bilinear finite element method,
%    since the data is sampled everywhere along the boundary lines,
%    rather than at a finite number of nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Gordon,
%    Blending-Function Methods of Bivariate and Multivariate Interpolation
%    and Approximation,
%    SIAM Journal on Numerical Analysis,
%    Volume 8, Number 1, March 1971, pages 158-177.
%
%    William Gordon and Charles Hall,
%    Transfinite Element Methods: Blending-Function Interpolation over
%    Arbitrary Curved Element Domains,
%    Numerische Mathematik,
%    Volume 21, Number 1, 1973, pages 109-129.
%
%    William Gordon and Charles Hall,
%    Construction of Curvilinear Coordinate Systems and Application to
%    Mesh Generation,
%    International Journal of Numerical Methods in Engineering,
%    Volume 7, 1973, pages 461-477.
%
%    Joe Thompson, Bharat Soni, Nigel Weatherill,
%    Handbook of Grid Generation,
%    CRC Press, 1999.
%
%  Parameters:
%
%    Input, real R, S, the (R,S) coordinates of the point to be
%    evaluated.
%
%    Input, integer N, the dimension of the vector space.
%
%    External, BOUND_RS, is a function which is given (R,S)
%    coordinates and an component value I, and returns XI, the value
%    of the I-th component of the N-vector at that point.  BOUND_RS
%    will only be called for "sides", that is, for values (R,S) where
%    at least one of R and S is either 0.0 or 1.0.  BOUND_RS has the
%    form:
%      function xi = bound_rs ( r, s, i )
%
%    Output, real X(N), the interpolated value at the point (R,S).
%
  for i = 1 : n
%
%  Get the I-th coordinate component at the four corners.
%
    x00 = bound_rs ( 0.0, 0.0, i );
    x01 = bound_rs ( 0.0, 1.0, i );
    x10 = bound_rs ( 1.0, 0.0, i );
    x11 = bound_rs ( 1.0, 1.0, i );
%
%  Get the I-th coordinate component at the sides.
%
    xr0 = bound_rs ( r, 0.0, i );
    xr1 = bound_rs ( r, 1.0, i );
    x0s = bound_rs ( 0.0, s, i );
    x1s = bound_rs ( 1.0, s, i );
%
%  Interpolate the I-th coordinate component of the interior point.
%
    x(i) = blend_112 ( r, s, x00, x01, x10, x11, xr0, xr1, x0s, x1s );

  end

  return
end


function [detvtx,pth] = SStat_mass_FDR2(pval,maskvtx,rate)
% [detvtx,pth] = SStat_mass_FDR2(pval,maskvtx,rate)
%
% Two-stage FDR approach to achieve tighter control of the FDR. This
% procedure is more powerful than the original FDR procedure implemented in
% SStat_mass_FDR.
%
% Input
% pval: P-values.
% maskvtx: Mask's vertices (1-based). Default [] (all vertices included).
% rate: Expected FDR (between 0 and 1).
%
% Output
% detvtx: Detected vertices (1-based).
% pth: FDR threshold.
%
% $Revision: 1.1 $  $Date: 2015/01/06 17:03:59 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:03:59 $
%    $Revision: 1.1 $
% References: Benjamini, Y., Krieger, A.M., Yekutieli, D. (2006). Adaptive
% linear step-up procedures that control the false discovery rate.
% Biometrika, 93, 491-507.
%
if nargin < 3
    rate = 0.05;
    if nargin < 2
        maskvtx = [];
    end
end;
nv0 = length(pval);
if isempty(maskvtx)
   maskvtx = 1:nv0;
end;
p = pval(maskvtx);
nv = length(p);
%% First stage (m0 estimation)
q0 = rate/(1+rate);
pth0 = SStat_mass_FDR(p,q0);
detv0 = maskvtx(p <= pth0);
ndetv0 = length(detv0);
m0 = nv-ndetv0;
%% Second stage
if (ndetv0 ~= 0) && (ndetv0 ~= nv)
    pth = SStat_mass_FDR(p,q0*nv/m0);
    detvtx = maskvtx(p <= pth);
else
    detvtx = detv0;
    pth = pth0;
end;


%% DEMO 18: Arbitrary axis of rotation
%
%
%
% Some modenr CT geometires are starting to be a bit more complex, one of
% the common things being arbitrary axis of rotation i.e. the detector and the
% source can move not in a circular path, but in a "spherical" path.
%
% In TIGRE this has been implemented by defining the rotation with 3
% angles, specifically the ZYZ configuration of Euler angles.
%
%  This demo shows how to use it.
%
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
% % Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize

clear;
close all;
%% Define Geometry
%
% VARIABLE                                   DESCRIPTION                    UNITS
%-------------------------------------------------------------------------------------
geo.DSD = 1536;                             % Distance Source Detector      (mm)
geo.DSO = 1000;                             % Distance Source Origin        (mm)
% Detector parameters
geo.nDetector=[512; 512];					% number of pixels              (px)
geo.dDetector=[0.8; 0.8]; 					% size of each pixel            (mm)
geo.sDetector=geo.nDetector.*geo.dDetector; % total size of the detector    (mm)
% Image parameters
geo.nVoxel=[128;128;128];                   % number of voxels              (vx)

% a bit smaller than usual because the demo includes a very big detector
% angle for showcase
geo.sVoxel=[256;256;256]/1.5;               % total size of the image       (mm)

geo.dVoxel=geo.sVoxel./geo.nVoxel;          % size of each voxel            (mm)
% Offsets
geo.offOrigin =[0;0;0];                     % Offset of image from origin   (mm)
geo.offDetector=[0; 0];                     % Offset of Detector            (mm)

% Auxiliary
geo.accuracy=0.5;                           % Accuracy of FWD proj          (vx/sample)

geo.mode='cone';

%% Define angles
numProjs = 100;

anglesY=linspace(0,2*pi,numProjs);
anglesZ2=anglesY;
anglesZ1=pi*sin(linspace(0,2*pi,numProjs));
angles=[anglesZ1;anglesY;anglesZ2];
%% Get Image

head=headPhantom(geo.nVoxel);

%% Project

projections=Ax(head,geo,angles);

plotProj(projections,(1:100)*pi/180); % angle information not right in the title
%% Reconstruct:

% Note, FDK will not work.

imgSIRT = SIRT(projections,geo, angles,50);
imgCGLS = CGLS(projections,geo, angles,10);

plotImg([head imgCGLS imgSIRT] ,'dim',3)


function [S,SE] = sample_edges(V,E,samples_per_edge)
  % SAMPLE_EDGES Compute samples_per_edge extra points along each edge in E
  % defined over vertices of V.
  %
  % S = sample_edges(V,E,samples_per_edge)
  %
  % Inputs:
  %   V  vertices over which edges are defined, # vertices by dim
  %   E  edge list, # edges by 2
  %   samples_per_edge  number of extra samples to be computed along edge not
  %     including start and end points
  % Outputs:
  %   S  sampled vertices, size less than # edges * (2+samples_per_edge) by dim,
  %   always begins with V so that E is also defined over S
  %
  %

  dim = size(V,2);

  % trivial case
  if(isempty(E))
    S = [];
    SE = [];
  elseif(samples_per_edge < 0)
    S = V;
    SE = [];
  else
    % fraction parameter
    t = linspace(0.0,1.0,samples_per_edge+2);
    % get rid of start and end points
    t = t(2:(end-1));
    % repeat for each coordinate
    t = reshape(repmat(t,dim,1),1,size(t,2)*dim);
    % repeat for each edge
    t = repmat(t,size(E,1),1);
    % repeat start coords

    sp = repmat(V(E(:,1),:),1,samples_per_edge);
    % repeat end coords
    ep = repmat(V(E(:,2),:),1,samples_per_edge);
    % lerp from start point to end point for each coordinate
    S = sp.*(1-t) + ep.*t;
    % reshape to list coordinates
    S = S';
    S = reshape(S,dim,prod(size(S))/dim)';
    S = [V;S];
    % Determine edges between samples
    E1 = repmat((1:(samples_per_edge-1))',size(E,1),1);
    E2 = repmat((2:(samples_per_edge))',size(E,1),1);
    off = size(V,1) + ...
      reshape( ...
        repmat( ...
          samples_per_edge*((1:size(E,1))-1), ...
          samples_per_edge-1, ...
          1), ...
        (samples_per_edge-1)*size(E,1),...
        1);
    SE = repmat(off,1,2) + [E1 E2];
    % end point edges
    SE = [ ...
      SE; ...
      E(:,1) size(V,1) + (1+samples_per_edge*((1:size(E,1))-1))'; ...
      E(:,2) size(V,1) + (samples_per_edge*((1:size(E,1))))'];
  end
end


function [elem2dof,elem2edge,edge,bdDof,freeDof] = dofP3(elem)
%% DOFP3 dof structure for P3 element.
%
%  [elem2dof,edge,bdDof] = DOFP3(elem) constructs the dof structure
%  for the quadratic element based on a triangle. elem2dof(t,i) is the
%  global index of the i-th dof of the t-th element.
%
%  The global indices of the dof is organized  according to the order of
%  nodes, edges and elements, namely, first give index number to the dofs
%  on nodes, then the dofs on edges, last the dofs on elements.
%
%  See also dofP2, dof3P3.
%
%  Created by Jie Zhou.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

N = max(max(elem)); NT = size(elem,1);

%% Data structure
totalEdge = uint32(sort([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])],2));
matlabversion = version;
if str2double(matlabversion(end-5:end-2)) > 2012
    [edge, i2, j] = unique(totalEdge,'rows','legacy');
else
    [edge, i2, j] = unique(totalEdge,'rows');
end
NE = size(edge,1);
elem2edge = reshape(j,NT,3);

%% Nodal dof
elem2dof = uint32(zeros(NT,10));
elem2dof(:,1:3) = elem;

%% Two dof on each edge
% edge 1
idx0 = (elem(:,3) > elem(:,2));
elem2dof(idx0,4) = N + 2*(elem2edge(idx0,1))-1;
elem2dof(idx0,5) = N + 2*(elem2edge(idx0,1));
elem2dof(~idx0,4)= N + 2*(elem2edge(~idx0,1));
elem2dof(~idx0,5)= N + 2*(elem2edge(~idx0,1))-1;
% edge 2
idx0 = (elem(:,3) > elem(:,1));
elem2dof(idx0,6) = N + 2*(elem2edge(idx0,2));
elem2dof(idx0,7) = N + 2*(elem2edge(idx0,2))-1;
elem2dof(~idx0,6) = N + 2*(elem2edge(~idx0,2))-1;
elem2dof(~idx0,7) = N + 2*(elem2edge(~idx0,2));
% edge 3
idx0 = (elem(:,2) > elem(:,1));
elem2dof(idx0,8) = N + 2*(elem2edge(idx0,3))-1;
elem2dof(idx0,9) = N + 2*(elem2edge(idx0,3));
elem2dof(~idx0,8) = N + 2*(elem2edge(~idx0,3));
elem2dof(~idx0,9) = N + 2*(elem2edge(~idx0,3))-1;

%% Element dof
elem2dof(:,10) = (N+2*NE+1:N+2*NE+NT)';

%% Boundary dof
i1(j(3*NT:-1:1)) = 3*NT:-1:1;
i1 = i1';
bdEdgeIdx = (i1 == i2);
isBdDof = false(N+2*NE+NT,1);
isBdDof(edge(bdEdgeIdx,:)) = true;   % boundary node
idx = find(bdEdgeIdx);
isBdDof(N+2*idx) = true;      % two dof on boundary edges
isBdDof(N+2*idx-1) = true;
bdDof = find(isBdDof);
 freeDof = find(~isBdDof);


function [Y,h]=glyph(X,r,c)
%Syntax: [Y,h]=glyph(X,r,c)
%__________________________
%
% 3D glyph visualization. The glyph is a convex deltahedron where each
% node is connected to its 6 nearest nodes.
%
% Y is the N-by-3 matrix whith the cartesian coordinates of the points on
%   the glyph.
% h returns a vector of tetrahedra handles. Each element of h is a handle
%   to the set of patches forming one tetrahedron. Type "help tetramesh"
%   for more info.
% X is the N-by-3 matrix whith the cartesian coordinates of the points on
%   the sphere.
% r is the range parameter.
% c is the color parameter.
%
%
% References:
%
% Sangole A., Knopf G. K. (2002): Representing high-dimensional data sets
% as close surfaces. Journal of Information Visualization 1: 111-119
%
% Sangole A., Knopf G. K. (2003): Geometric representations for
% high-dimensional data using a spherical SOFM. International Journal of
% Smart Engineering System Design 5: 11-20
%
% Sangole A., Knopf G. K. (2003): Visualization of random ordered numeric
% data sets using self-organized feature maps. Computers and Graphics 27:
% 963-976
%
% Sangole A. P. (2003): Data-driven Modeling using Spherical
% Self-organizing Feature Maps. Doctor of Philosophy (Ph.D.) Thesis.
% Department of Mechanical and Materials Engineering. Faculty of
% Engineering. The University of Western Ontario, London, Ontario, Canada.
%
%
% Remark:
%
% If no output is desired, the function plots the glyph.
%
%
% Archana P. Sangole, PhD., P.E. (TX chapter)
% School of Physical & Occupational Therapy
% McGill University
% 3654 Promenade Sir-William-Osler
% Montreal, PQ, H3G 1Y5
% e-mail: archana.sangole@mail.mcgill.ca
%
% CRIR, Rehabilitation Institute of Montreal
% 6300 Ave Darlington
% Montreal, PQ, H3S 2J5
% Tel: 514.340.2111 x2188
% Fax: 514.340.2154
%
%
% Alexandros Leontitsis, PhD
% Department of Education
% University of Ioannina
% 45110- Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 23-Mar-2006

% Add a center to the sphere, which is [0 0 0]
X=[X;zeros(1,3)];

% 3-dimensional Delaunay tessellation
tri=delaunay3(X(:,1),X(:,2),X(:,3));

% Remove the center
X(end,:)=[];

if nargin<2 | isempty(r)==1
    r=ones(length(X),1);
else
    % r must be a vector
    if min(size(r))>1
        error('r must be a vector.');
    end
    % The length of r should be equal to the length of X.
    if length(r)~=length(X)
        error('The length of r should be equal to the length of X.');
    end
    r=r(:);
end

if nargin<3 | isempty(c)==1
    c=r;
else
    % c must be a vector
    if min(size(c))>1
        error('c must be a vector.');
    end
    % The length of c should be equal to the length of X.
    if length(c)~=length(X)
        error('The length of c should be equal to the length of X.');
    end
    c=c(:);
end

% Go to spherical coordinates, ...
[theta,phi]=cart2sph(X(:,1),X(:,2),X(:,3));
% ... and compute the cartesian coordinates with the given r
[Y(:,1),Y(:,2),Y(:,3)]=sph2cart(theta,phi,r);

% Add a center to the glyph, which is [0 0 0]
Y=[Y;zeros(1,3)];
% Sort the vertices of each tereahedron ...
tri=sort(tri')';
% ... in order to calculate the color
i=1:length(tri);
cnew(i)=mean(c(tri(i,1:end-1))')';

% If no output is desired, plot the glyph
if nargout==0
    % Plot the glyph
    tetramesh(tri,Y,cnew,'FaceAlpha',1,'LineStyle','-');
    % Define the x axis
    xlim([-1.2 1.2])
    % Define the y axis
    ylim([-1.2 1.2])
    % Define the z axis
    zlim([-1.2 1.2])
    % Define the color axis
    caxis([1 1.2]);
    % Remove the axes
    axis off
    % Freeze aspect ratio properties to faciliate 3D rotation
    axis vis3d
% Else if the handle is desired
elseif nargout==2
    % Retrieve the handle
    h=tetramesh(tri,Y,cnew,'FaceAlpha',1,'LineStyle','none');
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = inversekinematic_Viper_s1300(robot, T)
%   Solves the inverse kinematic problem for the ADEPT Viper_S1300 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=inversekinematic_Viper_s1300 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('ADEPT', 'Viper_s1300');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function [q] = inversekinematic_Viper_s1300(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry at the reference for this robot, distance from the wrist to
%the end effector
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution, obtain theta1
% by geometric methods
q1 = atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this ABB robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

    %use either one algebraic method or the geometric
    %qtemp = solve_spherical_wrist(robot, q(:,i), T, 1, 'geometric'); %wrist up
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    %qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist down
    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'algebraic'); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%DH table parameters with which we calculate theta2, using
%geometric methods
L2=a(2);
L3=d(4);

%Offset distance between the centers of the reference systems of the links
%2 and 3
A1 = a(3);

%See geometry of the robot. Considering L4 calculate the offset between the
%centers of the reference systems of the links 2 and 3
L4 = sqrt(A1^2 + L3^2);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

%Distance between the system 1 to the wrist
r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));   %Theorem of the cosine

if ~isreal(gamma)
    disp('WARNING:inversekinematic_Viper_s1300: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta2. We add pi/2 to offset lags in our reference systems
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%DH table parameters with which we calculate theta3, using
%geometric methods
L2=a(2);
L3=d(4);

A1 = a(3);

%See geometry of the robot, like in the function q2
L4 = sqrt(A1^2 + L3^2);

%The delta angle is fixed because they are the lines that make up the gap
%between the links 2, 3 and 4
delta = real(acos((A1^2+L4^2-L3^2)/(2*A1*L4)));   %Theorem of the cosine

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

%Same as the function q2
r = sqrt(p1(1)^2 + p1(2)^2);

%Real angle between the links 2 and 3
ro = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));   %Theorem of the cosine

if ~isreal(ro)
   disp('WARNING:inversekinematic_Viper_s1300: the point is not reachable for this configuration, imaginary solutions');
   %ro = real(ro);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3. We add pi to offset lags in our reference systems
q3(1) = pi - ro - delta; %elbow up
q3(2) = pi + ro - delta; %elbow down

%remove complex solutions for q for the q1+pi solutions
function  qreal = arrange_solutions(q)
qreal=q(:,1:4);

%sum along rows if any angle is complex, for any possible solutions, then v(i) is complex
v = sum(q, 1);

for i=5:8,
    if isreal(v(i))
        qreal=[qreal q(:,i)]; %store the real solutions
    end
end

qreal = real(qreal);


function BED = calc_BED(paramS,varargin)
%BED = calc_BED(paramS)
%Ref: Comparison Between Mechanistic Radiobiological Modeling Vs.
%Fowler BED Equation in Evaluating Lung Cancer Radiotherapy Outcome
%for a Broad Range of Fractionation, J Jeong et al., AAPM 2017
%-----------------------------------------------------------------------
% INPUTS
% paramS : Parameter dictionary with fields:
%          d  - Fraction size
%          n  - No. fractions
%          T  - No. treatment days
%          alpha
%          abRatio
% Note : For a 3D dose distibution, calculate 3D BED by setting input
%        paramS.frxSize.val = doseArray3M/numFrx
%-----------------------------------------------------------------------
% AI 12/4/17
% AI 07/30/18 Updated to handle 3D dose distibution

%Define constants
Tk = paramS.Tk.val;         %Kick-off time of repopulation (days)
Tp =  paramS.Tp.val;        %Potential tumor doubling time (days)

alpha = paramS.alpha.val;
abRatio = paramS.abRatio.val;  %alpha/beta for tumor
d = paramS.frxSize.val;
n = paramS.numFractions.val;
if isfield(paramS,'treatmentDays')
    txDaysV = paramS.treatmentDays.val;
    %Check for numeric input
    if ~isnumeric(txDaysV)
        txDaysV = str2num(txDaysV);
    end
    %Otherwise assume function specified
    if isempty(txDaysV)
        txDaysV = eval([txDaysV,'(',num2str(n),')']);
    end
    T = txDaysV(end);
else
    % Default: Compute length of treatment assuming one fraction every
    % weekday with weekend breaks.
    T = floor(n/5)*7 + mod(n,5);
end

%Compute BED
BED = n.* d .* (1 + d./abRatio);
if T > Tk
  BED = BED -  log(2) * (T-Tk)/(alpha*Tp);
end

end


function [axis,angle]=rotation2axisangle(R)
cosq = 0.5*(R(1,1)+R(2,2)+R(3,3)-1);
if(cosq>1)
    cosq=1;
end
if(cosq<-1)
    cosq=-1;
end
angle = acos(cosq);
axis = zeros(3,1);
if(abs(angle)<1e-4)
elseif(abs(angle-pi)<1e-4)
    for i=1:3
        a = 0.5*(R(i,i)+1);
        if abs(a)<1e-4
            axis(i) = 0;
        else
            axis(i) = a^0.5;
        end
    end

    maxind = 1;
    if axis(2)>axis(1)
        maxind = 2;
    end
    if axis(3)>axis(2)
        maxind = 3;
    end
    if(maxind == 1)
        a1a2 = R(1,2) + R(2,1);
        a1a3 = R(1,3) + R(3,1);
        if(a1a2<0)
            axis(2) = -axis(2);
        end
        if(a1a3<0)
            axis(3) = -axis(3);
        end
    end
    if(maxind == 2)
        a1a2 = R(1,2) + R(2,1);
        a2a3 = R(2,3) + R(3,2);
        if(a1a2<0)
            axis(1) = -axis(1);
        end
        if(a2a3<0)
            axis(3) = -axis(3);
        end
    end
    if(maxind == 3)
        a1a3 = R(1,3) + R(3,1);
        a2a3 = R(2,3) + R(3,2);
        if(a1a3<0)
            axis(1) = -axis(1);
        end
        if(a2a3<0)
            axis(2) = -axis(2);
        end
    end
    axis = axis/norm(axis);
else
    axis(1) = R(3,2) - R(2,3);
    axis(2) = R(1,3) - R(3,1);
    axis(3) = R(2,1) - R(1,2);
    axis = axis/norm(axis);
end

end

github-repos/MATLAB/xuhuairuogu-V-REP-Simulation-Projects/V-REP-Simulation-Projects-841b944af4ea3a8fb250578d36434515f577f411/admittance control(adapted from an example in the book--A Systematic Approach to Learning Robot Programming with ROS)/iiwa14_kinematics/rotation2axisangle.m


% Estimation of the standard deviation of an image assuming that the noise
% is Gaussian white additive. The file estimates the standard deviation
% using the median filter on the fine scale subband of the wavelet
% decomposition.
 function sd_estimate=sdest(x)
 [ca,ch,cv,cd] = dwt2(x,'sym4','mode','sym');
 sd_estimate = mad(cd(:),1)/0.6745


%  Figure 10.72      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% Fig. 10.72
% Data for RTP Demo 3-3-99
% Data provided by Dr. Gwen van der Linden
% Data is from System Identification Studies
InputFlux=[3.460064464376177e-1 1.177299050104922e-1 2.838023866104041e-2;
   3.880303397347619e-11 8.024902450324316e-2 1.807231516460469e-2;
   8.004191616976514e-9 2.721604310757543e-3 3.171348842079633e-2];
M_inv=diag([1.000040130716728 5.557442686788876 13.63821806414694]);
Radiation=[5.47621193859299e-2 -8.570695054070524e-3 -8.296135532988507e-4...
      -4.536181077856052e-2;
   -8.570695054070524e-3 8.570946319867835e-3 -1.621311365067015e-7...
      -8.913466080455817e-8;
   -8.296135532988507e-4 -1.621311365067015e-7 8.299854517643017e-4...
      -2.097673289443245e-7];
Conduction=[3.559939609150268e-7 -1.113667477845243e-7 -1.976161155515125e-7...
      -4.701109757899004e-8;
   -1.113667477845243e-7 1.160207476868843e-2 -2.502736022145532e-3...
      -9.099227379795117e-3;
   -1.976161155515125e-7 -2.502736022145532e-3 6.37364815665867e-3...
      -3.870714518397587e-3];
ScaleTemp=diag([0.01 0.01 0.01 0.01]);

clf;
%
sim('fig10_71')
%plot(tout,r,'-');
%hold on;
%plot(tout,y,'--');
%xlabel('Time (sec)');
%ylabel('Temperature (K)');
%hold off;
%pause;
ii=240:876;
plot(tout(ii),r(ii),'-',tout(ii),y(ii,2),'--');
legend('r','y');
grid on;
xlabel('Time (sec)');
ylabel('Temperature (K)');
title('Fig. 10.72(a) Temperature tracking response');
pause;
hold off;
ii=240:876;
plot(tout(ii),u(ii),'-');
xlabel('Time (sec)');
ylabel('Lamp voltage (V)');
grid on;
legend('u');
title('Fig. 10.72(b) Control effort');


function [u,p,edgeC,A,eqn,info] = StokesisoP2P1(node,elem,pde,bdFlag,option)
%% STOKESISOP2P1 Stokes equation: isoP2-P1 modified Taylor-Hood elements.
%
%  [u,p] = STOKESisoP2P1(node,elem,pde,bdFlag) use constinous P1 element
%  on grid h and continous P1 element on grid H = 2*h to approximate
%  velocity u and pressure p, repectively.
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%   with
%       Dirichlet boundary condition        u = g_D  on \Gamma_D,
%       Neumann boundary condition du/dn - np = g_N  on \Gamma_N.
%
% Created by Ming Wang at Aug., 2012.
%
% See also StokesisoP2P0, StokesP2P1
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end

%% Refine grid for P1
nodeC = node; elemC = elem; bdFlagC = bdFlag;
[tempvar,edgeC] = dofP2(elemC);
[node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
NC = size(nodeC,1); NTC = size(elemC,1);
N = size(node,1);  NT = size(elem,1); Nu = N; Np = NC;

tic;
%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);
areaC = sum(reshape(area,NTC,4),2);

%% Assemble stiffness matrix for Laplace operator
A = sparse(Nu,Nu);
for i = 1:3
    for j = i:3
        Aij = (Dlambda(:,1,i).*Dlambda(:,1,j) + ...
            Dlambda(:,2,i).*Dlambda(:,2,j)).*area;
        if isfield(pde,'mu') && (pde.mu~=1)
            Aij = pde.mu*Aij;
        end
        if (j==i)
            A = A + sparse(elem(:,i),elem(:,j),Aij,N,N);
        else
            A = A + sparse([elem(:,i);elem(:,j)],[elem(:,j);elem(:,i)],...
                [Aij; Aij],N,N);
        end
    end
end
clear Aij
A = blkdiag(A,A);

%% Assemble the matrix for divergence operator
% idea: Basis on coarse grids can be expanded from fine grids, i.e.,
%       lambda_{i,c} = lambda_{i,f} + 1/2*sum_{j \ in V(i)} lambda_{j,f}
%        where V(i) is the index of points on edges surrounding point i.
Dx = sparse(N,N);
Dy = sparse(N,N);
for j = 1:3 % loop for u index
    Dx = Dx + sparse(elem(:),repmat(elem(:,j),3,1),...
                     repmat(1/3*Dlambda(:,1,j).*area,3,1),N,N);
    Dy = Dy + sparse(elem(:),repmat(elem(:,j),3,1),...
                     repmat(1/3*Dlambda(:,2,j).*area,3,1),N,N);
end
Bf = [-Dx -Dy];
Bv = Bf(1:NC,:);
Be = Bf(NC+1:end,:);
D = 1/2*icdmat(double(edgeC),[1,1]);
B = Bv + D'*Be;

%% Assemble right hand side by 4-points quadrature rule
f1 = zeros(Nu,1);
f2 = zeros(Nu,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,weight] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    ft1 = zeros(NT,3);
    ft2 = zeros(NT,3);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        % function values at quadrature points
        fp = pde.f(pxy);
        % evaluate fp outside.
        for j = 1:3
            ft1(:,j) = ft1(:,j) + fp(:,1).*lambda(p,j)*weight(p);
            ft2(:,j) = ft2(:,j) + fp(:,2).*lambda(p,j)*weight(p);
        end
    end
    ft1 = ft1.*repmat(area,1,3);
    ft2 = ft2.*repmat(area,1,3);
    f1 = accumarray(elem(:),ft1(:),[Nu 1]);
    f2 = accumarray(elem(:),ft2(:),[Nu 1]);
end

[AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesisoP2P1;

%% Record assembeling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(ufreeDof), return; end
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if length(f)+length(g) <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else          % Multigrid-type  solver for large size systems
        option.solver = 'asmg';
    end
end
solver = option.solver;

%% Solver
switch solver
    case 'direct'
        tic;
        bigA = [AD, BD'; ...
                BD, sparse(Np,Np)];
        bigF = [f; g];
        bigu = [u; p];
        bigFreeDof = [ufreeDof; 2*Nu+pDof];
        bigu(bigFreeDof) = bigA(bigFreeDof,bigFreeDof)\bigF(bigFreeDof);
        u = bigu(1:2*Nu);
        p = bigu(2*Nu+1:end);
        residual = norm(bigF - bigA*bigu);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'mg'
        option.solver  = 'WCYCLE';
        [u(ufreeDof),p,info] = mgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                        u(ufreeDof),p,elemC,ufreeDof,option);
    case 'asmg'
        [u(ufreeDof),p,info] = asmgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                          u,p,nodeC,elemC,bdFlagC,ufreeDof,option);
end

%% Post-process
if length(pDof)~=Np % p is unique up to a constant
    % impose the condition int(p)=0
    c = sum(mean(p(elemC),2).*areaC)/sum(areaC);
    p = p - c;
end

%% Output information
eqn = struct('A',AD,'B',BD,'f',f,'g',g,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdStokesisoP2P1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesisoP2P1
        %% Initial set up
        g = zeros(Np,1);
        u = zeros(2*Nu,1);
        p = zeros(Np,1);
        ufreeDof = (1:Nu)';
        pDof = (1:Np)';
        if ~exist('bdFlag','var'), bdFlag = []; end
        if ~isfield(pde,'g_D'), pde.g_D = []; end
        if ~isfield(pde,'g_N'), pde.g_N = []; end
        if ~isfield(pde,'g_R'), pde.g_R = []; end

        %% Part 1: Find Dirichlet dof and modify the matrix
        % Find Dirichlet boundary dof: fixedDof and pDof
        isFixedDof = false(Nu,1);
        if ~isempty(bdFlag) % case: bdFlag is not empty
            allEdge = [elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])];
            Dirichlet = allEdge((bdFlag(:) == 1),:);
            isFixedDof(Dirichlet(:)) = true;
            fixedDof = find(isFixedDof);
            ufreeDof = find(~isFixedDof);
        end
        if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N)
            fixedDof = findboundary(elem);
            isFixedDof(fixedDof) = true;
            ufreeDof = find(~isFixedDof);
        end
        if isempty(fixedDof) % pure Neumann boundary condition
            % pde.g_N could be empty which is homogenous Neumann boundary condition
            fixedDof = 1;
            ufreeDof = 2:Nu;    % eliminate the kernel by enforcing u(1) = 0;
        end

        % Modify the matrix
        % Build Dirichlet boundary condition into the matrix AD by enforcing
        % AD(fixedDof,fixedDof)=I, AD(fixedDof,ufreeDof)=0, AD(ufreeDof,fixedDof)=0.
        % BD(:,fixedDof) = 0 and thus BD'(fixedDof,:) = 0.
        bdidx = zeros(2*Nu,1);
        bdidx(fixedDof) = 1;
        bdidx(Nu+fixedDof) = 1;
        Tbd = spdiags(bdidx,0,2*Nu,2*Nu);
        T = spdiags(1-bdidx,0,2*Nu,2*Nu);
        AD = T*A*T + Tbd;
        BD = B*T;

        %% Part 2: Find boundary edges and modify the right hand side f and g
        % Find boundary edges: Neumann and Robin
        Neumann = []; Robin = []; %#ok<*NASGU>
        if ~isempty(bdFlag)
            allEdge = [elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])];
            Neumann = allEdge((bdFlag(:)==2)|(bdFlag(:) == 3),:);
            Robin = allEdge((bdFlag(:) == 3),:);
        end
        if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
            % no bdFlag, only pde.g_N or pde.g_R is given in the input
            [tempvar,Neumann] = findboundary(elem);
            if ~isempty(pde.g_R)
                Robin = Neumann;
            end
        end

        % Neumann boundary condition
        if ~isempty(pde.g_N) && ~isempty(Neumann) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
            [lambda,w] = quadpts1(3);
            nQuad = size(lambda,1);
            ve = node(Neumann(:,1),:) - node(Neumann(:,2),:); % length of edge
            edgeLength = sqrt(sum(ve.^2,2));
            % update RHS
            gex = zeros(size(Neumann,1),2);   % x-component
            gey = zeros(size(Neumann,1),2);   % y-component
            for pp = 1:nQuad
                pxy = lambda(pp,1)*node(Neumann(:,1),:)+lambda(pp,2)*node(Neumann(:,2),:);
                gp = pde.g_N(pxy);
                gex(:,1) = gex(:,1) + w(pp)*edgeLength.*gp(:,1)*lambda(pp,1);
                gex(:,2) = gex(:,2) + w(pp)*edgeLength.*gp(:,1)*lambda(pp,2);
                gey(:,1) = gey(:,1) + w(pp)*edgeLength.*gp(:,2)*lambda(pp,1);
                gey(:,2) = gey(:,2) + w(pp)*edgeLength.*gp(:,2)*lambda(pp,2);
            end
            f1(1:N) = f1(1:N) + accumarray(Neumann(:), gex(:),[N,1]);
            f2(1:N) = f2(1:N) + accumarray(Neumann(:), gey(:),[N,1]);
        end
        f = [f1; f2];
        % The case non-empty Neumann but g_N=[] corresponds to the zero flux
        % boundary condition on Neumann edges and no modification is needed.

        % Dirichlet boundary conditions
        if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
            u1 = zeros(Nu,1);
            u2 = zeros(Nu,1);
            uD = pde.g_D(node(fixedDof,:));
            u1(fixedDof) = uD(:,1);
            u2(fixedDof) = uD(:,2);
            u = [u1;u2];
            f = f - A*u;  % bring affect of nonhomgenous Dirichlet bd condition
            g = g - B*u;  % to the right hand side
            g = g - mean(g);
            f(fixedDof)    = u1(fixedDof);
            f(fixedDof+Nu) = u2(fixedDof);
        end
        % The case non-empty Dirichlet but g_D=[] corresponds to the zero Dirichlet
        % boundary condition and no modification is needed.

        % modfiy pressure dof for pure Dirichlet
        if isempty(Neumann)
            pDof = (1:Np-1)';
        end

        ufreeDof = [ufreeDof; Nu+ufreeDof];
    end % end of function getbdStokesisoP2P1
end


% FactorMarginalization Sums given variables out of a factor.
%   B = FactorMarginalization(A,V) computes the factor with the variables
%   in V summed out. The factor data structure has the following fields:
%       .var    Vector of variables in the factor, e.g. [1 2 3]
%       .card   Vector of cardinalities corresponding to .var, e.g. [2 2 2]
%       .val    Value table of size prod(.card)
%
%   The resultant factor should have at least one variable remaining or this
%   function will throw an error.
%
%   See also FactorProduct.m, IndexToAssignment.m, and AssignmentToIndex.m
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function B = FactorMarginalization(A, V)

% Check for empty factor or variable list
if (isempty(A.var) || isempty(V)), B = A; return; end;

% Construct the output factor over A.var \ V (the variables in A.var that are not in V)
% and mapping between variables in A and B
[B.var, mapB] = setdiff(A.var, V);

% Check for empty resultant factor
if isempty(B.var)
    %error('Error: Resultant factor has empty scope');
    B.var = [];
    B.card = [];
    B.val = [];
    return;
end;

% Initialize B.card and B.val
B.card = A.card(mapB);
B.val = zeros(1,prod(B.card));

% Compute some helper indices
% These will be very useful for calculating C.val
% so make sure you understand what these lines are doing
assignments = IndexToAssignment(1:length(A.val), A.card);
indxB = AssignmentToIndex(assignments(:, mapB), B.card);

for i = 1:length(A.val),
    B.val(indxB(i)) = B.val(indxB(i)) + A.val(i);
end;

end


function c=ref_edgt(f,g,a,M)
%REF_EDGT   Reference Even Discrete Gabor transform
%   Usage  c=ref_edgt(f,g,a,M);
%
%   The input window must be odd-centered.

L=size(f,1);
W=size(f,2);

N=L/a;
M=L/b;

F=zeros(L,M*N);

l=(0:L-1)';
for n=0:N-1
  for m=0:M-1
    F(:,1+m+n*M)=exp(2*pi*i*m.*(l+.5)*b/L).*circshift(g,n*a);
  end;
end;

c=F'*f;


function [e, edata, eprior] = gperr(net, x, t)
%GPERR	Evaluate error function for Gaussian Process.
%
%	Description
%	E = GPERR(NET, X, T) takes a Gaussian Process data structure NET
%	together  with a matrix X of input vectors and a matrix T of target
%	vectors, and evaluates the error function E. Each row of X
%	corresponds to one input vector and each row of T corresponds to one
%	target vector.
%
%	[E, EDATA, EPRIOR] = GPERR(NET, X, T) additionally returns the data
%	and hyperprior components of the error, assuming a Gaussian prior on
%	the weights with mean and variance parameters PRMEAN and PRVARIANCE
%	taken from the network data structure NET.
%
%	See also
%	GP, GPCOVAR, GPFWD, GPGRAD
%

%	Copyright (c) Ian T Nabney (1996-2001)

errstring = consist(net, 'gp', x, t);
if ~isempty(errstring);
  error(errstring);
end

cn = gpcovar(net, x);

edata = 0.5*(sum(log(eig(cn, 'nobalance'))) + t'*inv(cn)*t);

% Evaluate the hyperprior contribution to the error.
% The hyperprior is Gaussian with mean pr_mean and variance
% pr_variance
if isfield(net, 'pr_mean')
  w = gppak(net);
  m = repmat(net.pr_mean, size(w));
  if size(net.pr_mean) == [1 1]
    eprior = 0.5*((w-m)*(w-m)');
    e2 = eprior/net.pr_var;
  else
    wpr = repmat(w, size(net.pr_mean, 1), 1)';
    eprior = 0.5*(((wpr - m').^2).*net.index);
    e2 = (sum(eprior, 1))*(1./net.pr_var);
  end
else
  e2 = 0;
  eprior = 0;
end

e = edata + e2;


function [xEst,PEst]=batchLSLinMeasLinDyn(z,H,F,R,kD,Q,numMeas)
%%BATCHLSLINMEASLINDYN Perform batch least squares state estimation under a
%                linear measurement model and a linear dynamic model with
%                optional process noise.
%
%INPUTS: z The zDim X N matrix of measurements for the whole batch. It is
%          assumed that the measurements have the same dimensionality over
%          the batch. If an empty matrix is passed, then it is assumed that
%          the user only wants the covariance matrix and not xEst.
%        H The zDim X xDim X N hypermatrix of measurement matrices such
%          that H(:,:,k)*x+w is the measurement at time k, where x is the
%          state and w is zero-mean Gaussian noise with covariance matrix
%          R(:,:,k). Alternatively, if all of the measurement matrices are
%          the same, one can just pass a single zDim X xDim matrix.
%        F An xDim X xDim X (N-1) hypermatrix of  matrices. The state at
%          discrete-time k+1 is modeled as F(:,:,k) times the state at time
%          k plus zero-mean Gaussian process noise with covariance matrix
%          Q(:,:,k). Alternatively, if all of the state transition matrices
%          are the same, one can just pass a single xDim X xDim matrix.
%          Note that all of the F matrices must be invertible if kD is not
%          equal to one.
%        R The zDim X zDim X N hypermatrix of measurement covariance
%          matrices. Alternatively, if all of the measurement covariance
%          matrices are the same, one can just pass a single zDimXzDim
%          matrix.
%       kD The discrete time-step for which the covariance matrix of an
%          estimate is desired, where z(:,1) is at discrete time-step 1
%          (not 0).
%        Q The xDim X xDim X (N-1) hypermatrix of process noise covariance
%          matrices. Alternatively, if all of the process noise covariance
%          matrices are the same, one can just pass a single xDimXxDim
%          matrix. If an empty matrix is passed for Q, then the estimation
%          is performed assuming there is no process noise.
%  numMeas If an empty matrix is passed for z, then the numMeas parameter
%          must be passed indicating the number of measurements in the
%          batch. Otherwise, this parameter is
%                ignored.
%
%OUTPUTS: xEst The batch state estimate at step kD, unless an empty matrix
%              was passed for z, in which case xEst is empty.
%         PEst A covariance matrix estimate that goes with the state
%              estimate.
%
%The algorithm is an implementation of the method of Section 3.3.2 of [1]
%for a linear dynamic model. Note that the cases in equation 28 to 32 of
%the paper did not cover all of the possibilities, so the correct
%generalization had to be derived from E[epsilon_{p,k}*epsilon_{q,k}'].
%
%REFERENCES:
%[1] A. B. Poore, B. J. Slocumb, B. J. Suchomel, F. H. Obermeyer, S. M.
%    Herman, and S. M. Gadaleta, "Batch maximum likelihood (ML) and maximum
%    a posteriori (MAP) estimation with process noise for tracking
%    applications," in Proceedings of SPIE: Signal and Data Processing of
%    Small Targets, vol. 5204, San Diego, CA, 3 Aug. 2003, pp. 188-199.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(~isempty(z))
    numMeas=size(z,2);
end

xDim=size(F,1);
zDim=size(R,1);

if(size(H,3)==1)
    HMats=repmat(H,[1,1,numMeas]);
else
    HMats=H;
end

if(size(F,3)==1)
    F=repmat(F,[1,1,numMeas-1]);
end

if(size(R,3)==1)
    R=repmat(R,[1,1,numMeas]);
end

if(isempty(Q))
    Q=zeros(xDim,xDim,numMeas);
elseif(size(Q,3)==1)
    Q=repmat(Q,[1,1,numMeas-1]);
end

transMats=getTransMats(F);

WMat=zeros(zDim*numMeas,zDim*numMeas);
for p=1:numMeas
    pIdxMin=(p-1)*zDim+1;
    pIdxMax=p*zDim;
    pSpan=pIdxMin:pIdxMax;
    for q=1:numMeas
        qIdxMin=(q-1)*zDim+1;
        qIdxMax=q*zDim;
        qSpan=qIdxMin:qIdxMax;
        if(p<kD&&q<kD)
            CumMat=zeros(xDim,xDim);

            for i=(max(p,q)+1):kD
                CumMat=CumMat+transMats(:,:,p,i)*Q(:,:,i-1)*transMats(:,:,q,i)';
            end

            WMat(pSpan,qSpan)=HMats(:,:,p)*CumMat*HMats(:,:,q)'+KDelta(p-q)*R(:,:,p);
        elseif(p>kD&&q>kD)
            CumMat=zeros(xDim,xDim);

            for i=(kD+1):min(p,q)
                CumMat=CumMat+transMats(:,:,p,i)*Q(:,:,i-1)*transMats(:,:,q,i)';
            end

            WMat(pSpan,qSpan)=KDelta(p-q)*R(:,:,p)+HMats(:,:,p)*CumMat*HMats(:,:,q)';
        elseif(p==kD&&q==kD)
            WMat(pSpan,qSpan)=R(:,:,p);
        end
    end
end

%The propagated measurement matrices.
bigH=zeros(zDim*numMeas,xDim);
for curMeas=1:numMeas
    idxMin=(curMeas-1)*zDim+1;
    idxMax=curMeas*zDim;

    bigH(idxMin:idxMax,:)=HMats(:,:,curMeas)*transMats(:,:,curMeas,kD);
end

WInv=inv(WMat);
PEstInv=bigH'*WInv*bigH;
PEst=inv(PEstInv);
if(isempty(z))
    xEst=[];
else
    xEst=PEstInv\bigH'*(WMat\z(:));
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 [pitch, roll, yaw] = q2att(qnb)
    q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4);
    q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4);
    q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4);
    q44 = qnb(4)*qnb(4);
    C12=2*(q23-q14);
    C22=q11-q22+q33-q44;
    C31=2*(q24-q13); C32=2*(q34+q12); C33=q11-q22-q33+q44;

    pitch = asind(C32);
    roll = atan2d(-C31,C33);
    yaw = atan2d(C12,C22);
    yaw = yaw + (yaw<0)*360;
end


function [fx] = evolution0bisND(x,theta,u,in)
% 0-ToM's evolution function (without doubled hidden states)
% function [fx] = evolution0bisND(x,theta,u,in)
% 0-ToM is simply tracking the log-odds of P(o=1), where o is the
% opponent's action. This variable is updated according to a Laplace-Kalman
% filter, yielding 2 sufficient statistics, m and V. In this scheme, the
% only evolution param (theta) is 0-ToM's prior volatity about her
% opponent's log-odds.
% IN:
%   - x: sufficient statistics of log-odds of P(o=1):
%       x(1)= E[log-odds]
%       x(2)= log V[log-odds] (log-scale for numerical reasons)
%   - theta: 0-ToM's prior (log-) volatity
%   - u: u(1)= last opponent's move (o)
%   - in: [useless here]
% OUT:
%   - fx: updated sufficient statistics of log-odds of P(o=1)

if isempty(u)||isnan(u(1)) % missed trial
    fx = x; % no update
else % trial OK
    % -- deal with superceding competition with other generative models --
    % [See, e.g., f_metaTom.m]
    try
        w = inF.metaweight;
    catch
        w = 1;
    end
    % -- learning rule --
    m0 = x(1); % current E[log-odds]
    V0 = exp(x(2)); % current V[log-odds]
    p0 = VBA_sigmoid(m0); % current estimate of P(o=1)
    volatility = exp(theta(1));
    V = 1./((1./(volatility+V0))+w*p0*(1-p0)); % updated V[log-odds]
    m = m0 + w*V*(u(1)-p0); % updated E[log-odds] (Laplace-Kalman update rule)
    % wrap-up
    fx = [VBA_sigmoid(VBA_sigmoid(m),'inverse',true);log(V)]; % for numerical purposes
end


% confusion_matrix(1,1) = fraction of true 1 classified as 1
% confusion_matrix(1,2) = fraction of true 1 classified as 2
% confusion_matrix(1,3) = fraction of true 1 classified as unknown
% confusion_matrix(2,1) = fraction of true 2 classified as 1
% confusion_matrix(2,2) = fraction of true 2 classified as 2
% confusion_matrix(2,3) = fraction of true 2 classified as unknown
function confusion_matrix = ComputeConfusionMatrix(hs,diff_logprob,maxdiff_logprob1,mindiff_logprob2)

confusion_matrix = nan(2,3);
hsPr1 = diff_logprob < maxdiff_logprob1;
hsPr2 = diff_logprob > mindiff_logprob2;
n1 = nnz(hs==1);
n2 = nnz(hs==2);

confusion_matrix(1,1) = nnz(hsPr1(hs==1)) / n1;
confusion_matrix(1,2) = nnz(hsPr2(hs==1)) / n1;
confusion_matrix(1,3) = 1 - confusion_matrix(1,1) - confusion_matrix(1,2);

confusion_matrix(2,1) = nnz(hsPr1(hs==2)) / n2;
confusion_matrix(2,2) = nnz(hsPr2(hs==2)) / n2;
confusion_matrix(2,3) = 1 - confusion_matrix(2,1) - confusion_matrix(2,2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Setting up advection-diffusion solver
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [C,laplacian_C] = please_Update_Adv_Diff_Concentration_Flux_Limiter_FV(C,dt,dx,dy,uX,uY,k)

% C:     concentration
% dt:    time-step
% dx,dy: spatial steps in x and y, respectively
% uX:    x-Component of Velocity
% uY:    y-Component of Velocity
% k:     diffusion coefficient

% Compute Fluxes (Note: these calculations could be parallalized)
selection = 'superbee';
Fx = give_Necessary_Fluxes(C,dx,uX,'x',selection,dt); % Fluxes in x
Fy = give_Necessary_Fluxes(C,dy,uY,'y',selection,dt); % Fluxes in y

% "forward difference of fluxes in x"
F2x = [Fx(:,2:end) Fx(:,1)];
F1x = [Fx(:,end) Fx(:,1:end-1)];
diffX = 0.5/dx*( F2x - F1x );

% "forward differences of fluxes in y"
F2y = [Fy(2:end,:); Fy(1,:)];
F1y = [Fy(end,:); Fy(1:end-1,:)];
diffY = 0.5/dy*( F2y - F1y );

diffY=0;

% Compute 2nd Derivative Terms
Cxx = DD(C,dx,'x');
Cyy = DD(C,dy,'y');

% Forms Laplacian
laplacian_C = Cxx+Cyy;

% UPWIND
C = C - dt * ( diffX + diffY ) + dt*( k*laplacian_C );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Computes derivative based on sign of Velocity, u, using UPWIND
% approach
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C_z = give_Necessary_Fluxes(C,dz,uZ,string,selection,dt)

C_z = zeros(size(C));
len = length(uZ(:,1));
signs = sign(uZ);

if strcmp(string,'x')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %left side of grid
        if signs(i,1) <= 0
            r = ( C(i,2) - C(i,1) ) / ( C(i,1) - C(i,len) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,1) =  uZ(i,1)*C(i,2) ;%  + 0.5*abs( uZ(i,1) )*( 1 - abs( 0.5*dt*uZ(i,1)/dz ) )*phi*( C(i,2) - C(i,len) );
        else
            r = ( C(i,1) - C(i,len) ) / ( C(i,2) - C(i,1) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,1) =  uZ(i,1)*C(i,len) ;%+ 0.5*abs( uZ(i,1) )*( 1 - abs( 0.5*dt*uZ(i,1)/dz ) )*phi*( C(i,2) - C(i,len) );
        end

        %right side of grid
        if signs(len,1) <= 0
            r = ( C(i,1) - C(i,len) ) / ( C(i,len) - C(i,len-1) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,len) =  uZ(i,len)*C(i,1)  ;%   + 0.5*abs( uZ(i,len) )*( 1 - 0.5*abs( dt*uZ(i,len)/dz ) )*phi*( C(i,1) - C(i,len-1) );
        else
            r = ( C(i,len) - C(i,len-1) ) / ( C(i,1) - C(i,len) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,len) =  uZ(i,len)*C(i,len-1);% + 0.5*abs( uZ(i,len) )*( 1 - 0.5*abs( dt*uZ(i,len)/dz ) )*phi*( C(i,1) - C(i,len-1) );
        end

    end
    %Standard Upwind
    for i=1:len
        for j=2:len-1
            if signs(i,j) <= 0
                r = ( C(i,j+1) - C(i,j) ) / ( C(i,j) - C(i,j-1) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(i,j) = uZ(i,j)*C(i,j+1);% + 0.5*abs( uZ(i,j) )*( 1 - 0.5*abs( dt*uZ(i,j)/dz ) )*phi*( C(i,j+1) - C(i,j-1) );
            else
                r = ( C(i,j) - C(i,j-1) ) / ( C(i,j+1) - C(i,j) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(i,j) = uZ(i,j)*C(i,j-1);% + 0.5*abs( uZ(i,j) )*( 1 - 0.5*abs( dt*uZ(i,j)/dz ) )*phi*( C(i,j+1) - C(i,j-1) );
            end
        end
    end

    % Ends x-Direction calculation %

elseif strcmp(string,'y')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %bottom of grid
        if signs(1,i) <= 0
            r = ( C(2,i) - C(1,i) ) / ( C(1,i) - C(len,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(1,i) =  uZ(1,i)*C(2,i)   + 0.5*abs( uZ(1,i) )*( 1 - abs( 0.5*dt*uZ(1,i)/dz ) )*phi*( C(2,i) - C(len,i) );
        else
            r = ( C(1,i) - C(len,i) ) / ( C(2,i) - C(1,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(1,i) =  uZ(1,i)*C(len,i) + 0.5*abs( uZ(1,i) )*( 1 - abs( 0.5*dt*uZ(1,i)/dz ) )*phi*( C(2,i) - C(len,i) );
        end

        %top of grid
        if signs(len,1) <= 0
            r = ( C(1,i) - C(len,i) ) / ( C(len,i) - C(len-1,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(len,i) =  uZ(len,i)*C(1,i)       + 0.5*abs( uZ(len,i) )*( 1 - abs( 0.5*dt*uZ(len,i)/dz ) )*phi*( C(1,i) - C(len-1,i) );
        else
            r = ( C(len,i) - C(len-1,i) ) / ( C(1,i) - C(len,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(len,i) =  uZ(len,i)*C(len-1,i)   + 0.5*abs( uZ(len,i) )*( 1 - abs( 0.5*dt*uZ(len,i)/dz ) )*phi*( C(1,i) - C(len-1,i) );
        end

    end

    %Standard Upwind
    for i=1:len
        for j=2:len-1
            if signs(j,i) <= 0
                r = ( C(j+1,i) - C(j,i) ) / ( C(j,i) - C(j-1,i) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(j,i) = uZ(j,i)*C(j+1,i) + 0.5*abs( uZ(j,i) )*( 1 - abs( 0.5*dt*uZ(j,i)/dz ) )*phi*( C(j+1,i) - C(j-1,i) );
            else
                r = ( C(j,i) - C(j-1,i) ) / ( C(j+1,i) - C(j,i) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(j,i) = uZ(j,i)*C(j-1,i) + 0.5*abs( uZ(j,i) )*( 1 - abs( 0.5*dt*uZ(j,i)/dz ) )*phi*( C(j+1,i) - C(j-1,i) );
            end
        end
    end

    % Ends y-Direction calculation %

else

    fprintf('\n\n\n ERROR IN FUNCTION FOR COMPUTING FLUX LIMITERS\n');

end

clear signs; clear len;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Finds CENTERED finite difference approximation to 2ND
% DERIVATIVE in z direction, specified by input and 'string'
% Note: It automatically accounts for periodicity of the domain.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function u_zz = DD(u,dz,string)

% u:      velocity
% dz:     spatial step in "z"-direction
% string: specifies which 2ND derivative to take (to enforce periodicity)

len = length(u(:,1));

if strcmp(string,'x')

    %For periodicity on ends
    u_zz(:,1) =  ( u(:,2) - 2*u(:,1)   + u(:,len) )   / (dz^2);
    u_zz(:,len)= ( u(:,1) - 2*u(:,len) + u(:,len-1) ) / (dz^2);

    %Standard Upwind Scheme (Centered Difference)
    for j=2:len-1
        u_zz(:,j) = ( u(:,j+1) - 2*u(:,j) + u(:,j-1) ) / (dz^2);
    end

elseif strcmp(string,'y')

    %For periodicity on ends
    u_zz(1,:) =  ( u(2,:) - 2*u(1,:)   + u(len,:) )   / (dz^2);
    u_zz(len,:)= ( u(1,:) - 2*u(len,:) + u(len-1,:) ) / (dz^2);

    %Standard Upwind Scheme (Centered Difference)
    for j=2:len-1
        u_zz(j,:) = ( u(j+1,:) - 2*u(j,:) + u(j-1,:) ) / (dz^2);
    end

else

    fprintf('\n\n\n ERROR IN FUNCTION DD FOR COMPUTING 2ND DERIVATIVE\n');
    fprintf('Need to specify which desired derivative, x or y.\n\n\n');

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes flux limiter with choice of which one
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function phi = please_Give_Flux_Limiter(r,selection)

if strcmp(selection,'superbee')
    max1 = max( min(1,2*r),min(2,r) );
    phi = max(0,max1);
elseif strcmp(selection,'vanLeer')
    phi = ( r + abs(r) ) / ( 1 + abs(r) );
else
    fprintf('\n\n');
    error('NEED TO CHOOSE AN APPROPRIATE FLUX LIMITER');
end


classdef MOEADM2M_F5 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing MOEA/D-M2M

%------------------------------- Reference --------------------------------
% H. Liu, F. Gu, and Q. Zhang, Decomposition of a multiobjective
% optimization problem into a number of simple multiobjective subproblems,
% IEEE Transactions on Evolutionary Computation, 2014, 18(3): 450-455.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = X(:,2:end) - repmat(sin(pi/2*X(:,1)),1,size(X,2)-1);
            g = 2*abs(cos(pi*X(:,1))).*sum(-0.9*t.^2+abs(t).^0.6,2);
            PopObj(:,1) = (1+g).*X(:,1);
            PopObj(:,2) = (1+g).*(1-sqrt(X(:,1)));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


% Count the number of self-loops in the graph
%
% INPUT: adjacency matrix, nxn
% OUTPUT: integer, number of self-loops
%
% Note: in the adjacency matrix representation loops appear as non-zeros on the diagonal
% GB: last updated, Sep 20 2012

function sl=selfLoops(adj)

sl=sum(diag(adj));


function [out] = saturation_8(p1,p2,S,Smax,In)
%saturation_8

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Saturation excess flow from a store with different degrees
%               of saturation (min-max linear variant)
% Constraints:  -
% @(Inputs):    p1   - minimum fraction contributing area [-]
%               p2   - maximum fraction contributing area [-]
%               S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               In   - incoming flux [mm/d]

out = (p1+(p2-p1)*S/Smax)*In;

end


function M = getmassmat3(node,elem2dof,volume,type,K)
%% GETMASSMAT Get mass matrix of the finite element space
%
% M = GETMASSMAT(node,elem2dof,volume,type,K) get mass matrix of the finite element
% space specified by elemType.
%
% The type can be:
% - 'P1': full mass matrix for P1 element
% - 'lump': lumped mass matrix for P1 element

N = size(node,1);
NT = length(volume);
Ndof = double(max(elem2dof(:)));

%% Coefficients and default type
if ~exist('type','var'), type = 'P1'; end
if ~exist('K','var'), K = []; end

%% Assembling
n = size(elem2dof,2);
switch n
    case 4  % P1 element
    if strcmp(type,'lump')
        %% Assemble the mass matrix by the mass lumping
        M = accumarray([elem2dof(:,1);elem2dof(:,2);elem2dof(:,3);elem2dof(:,4)],...
                       [volume;volume;volume;volume]/4,[N,1]);
        if exist('K','var') && ~isempty(K) && ~isnumeric(K) % K is a function
            M = K(node).*M;
        elseif exist('K','var') && ~isempty(K) && isnumeric(K) && size(K,1) == N
            M = K.*M;
        end
        M = spdiags(M,0,N,N);
    else
        %% Assemble the full mass matrix
        if exist('K','var') && ~isempty(K) && ~isnumeric(K) % K is a function
            center = (node(elem2dof(:,1),:) + node(elem2dof(:,2),:) + ...
                      node(elem2dof(:,3),:) + node(elem2dof(:,4),:))/4;
            volume = K(center).*volume;
        elseif exist('K','var') && ~isempty(K) && isnumeric(K) && size(K,1) == NT
            volume = K.*volume;
        end
        M = sparse(N,N);
        for i = 1:4
            for j = i:4
                ii = double(elem2dof(:,i));
                jj = double(elem2dof(:,j));
                if (j==i)
                    M = M + sparse(ii,jj,volume/10,N,N);
                else
                    M = M + sparse([ii;jj],[jj;ii],[volume/20; volume/20],N,N);
                end
            end
        end
    end
    case 10 % P2 element
        %% Assemble the full mass matrix using nodal basis
        % indexing follows Poisson3P2
        [lambda, w] = quadpts3(2);
        nQuad = size(lambda,1);
        ii = zeros(55*NT,1); % 55: # of upper triangular entries
        jj = zeros(55*NT,1);
        sM = zeros(55*NT,1);
        phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
        phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
        phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
        phi(:,4) = lambda(:,4).*(2*lambda(:,4)-1);
        phi(:,5) = 4*lambda(:,1).*lambda(:,2);
        phi(:,6) = 4*lambda(:,1).*lambda(:,3);
        phi(:,7) = 4*lambda(:,1).*lambda(:,4);
        phi(:,8) = 4*lambda(:,2).*lambda(:,3);
        phi(:,9) = 4*lambda(:,2).*lambda(:,4);
        phi(:,10)= 4*lambda(:,3).*lambda(:,4);

        index = 0;
        for i = 1:10
            for j = i:10
                Mij = 0;
                for p = 1:nQuad; Mij = Mij + w(p)*phi(p,i).*phi(p,j); end
                Mij = Mij.*volume;
                ii(index+1:index+NT) = double(elem2dof(:,i));
                jj(index+1:index+NT) = double(elem2dof(:,j));
                sM(index+1:index+NT) = Mij;
                index = index + NT;
            end
        end
        diagIdx = (ii == jj);   upperIdx = ~diagIdx;
        M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
        MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
        M = M + MU + MU';

end


function [tfr,rtfr,hat] = tfrrpwv(x,t,N,h,trace);
%TFRRPWV Reassigned  pseudo Wigner-Ville distribution.
%	[TFR,RTFR,HAT] = TFRRPWV(X,T,N,H,TRACE)
%	computes the pseudo Wigner-Ville distribution
%	and its reassigned version.
%
%	X     : analysed signal,
%	T     : the time instant(s)      (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	H     : frequency smoothing window, H(0) being forced to 1
%	                                 (default : Hamming(N/4)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR,  : time-frequency representation and its reassigned
%	RTFR    version. When called without output arguments,
%	        TFRRPWV runs TFRQVIEW.
%	HAT   : Complex matrix of the reassignment vectors.
%
%	Example:
%	 sig=fmlin(128,0.1,0.4); t=1:2:128;
%	 h=tftb_window(17,'Kaiser'); tfrrpwv(sig,t,64,h,1);
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, May-July 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (nargin < 1),
 error('At least 1 parameter is required');
elseif (nargin <= 2),
 N=xrow;
end;

hlength=floor(N/4);
if (rem(hlength,2)==0),
 hlength=hlength+1;
end;

if (nargin == 1),
 t=1:xrow; h = tftb_window(hlength); trace=0;
elseif (nargin == 2)|(nargin == 3),
 h = tftb_window(hlength); trace=0;
elseif (nargin == 4),
 trace = 0;
end;

if (N<0),
 error('N must be greater than zero');
end;
[trow,tcol] = size(t);
if (xcol~=1),
 error('X must have only one column');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2; h=h/h(Lh+1);
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

if (tcol==1),
 Dt=1;
else
 Deltat=t(2:tcol)-t(1:tcol-1);
 Mini=min(Deltat); Maxi=max(Deltat);
 if (Mini~=Maxi),
  error('The time instants must be regularly sampled.');
 else
  Dt=Mini;
 end;
 clear Deltat Mini Maxi;
end;

tfr= zeros(N,tcol); tf2= zeros(N,tcol);
if trace, disp('Pseudo Wigner-Ville distribution'); end;
Dh=dwindow(h);
for icol=1:tcol,
 ti= t(icol); taumax=min([ti-1,xrow-ti,round(N/2)-1,Lh]);
 tau=-taumax:taumax; indices= rem(N+tau,N)+1;
 if trace, disprog(icol,tcol,10); end;
 tfr(indices,icol)= h(Lh+1+tau).*x(ti+tau).*conj(x(ti-tau));
 tf2(indices,icol)=Dh(Lh+1+tau).*x(ti+tau).*conj(x(ti-tau));
 tau=round(N/2);
 if (ti<=xrow-tau)&(ti>=tau+1)&(tau<=Lh),
  tfr(tau+1,icol) = 0.5 * ( h(Lh+1+tau) * x(ti+tau,1) * conj(x(ti-tau,xcol))  + ...
                            h(Lh+1-tau) * x(ti-tau,1) * conj(x(ti+tau,xcol))) ;
  tf2(tau+1,icol) = 0.5 * (Dh(Lh+1+tau) * x(ti+tau,1) * conj(x(ti-tau,xcol))  + ...
                           Dh(Lh+1-tau) * x(ti-tau,1) * conj(x(ti+tau,xcol))) ;
 end;
end ;
tfr= real(fft(tfr));
tf2=imag(fft(tf2));
tfr=tfr(:);tf2=tf2(:);
avoid_warn=find(tfr~=0);
tf2(avoid_warn)=round(N*tf2(avoid_warn)./tfr(avoid_warn)/(2.0*pi));
%tf2= round(N*imag(fft(tf2))./tfr/(2.0*pi));
if trace, fprintf ('\nreassignment: \n'); end;
tfr=reshape(tfr,N,tcol);
tf2=reshape(tf2,N,tcol);

rtfr= zeros(N,tcol);
Ex=mean(abs(x(min(t):max(t))).^2); Threshold=1.0e-6*Ex;
for icol=1:tcol,
 if trace, disprog(icol,tcol,10); end;
 for jcol=1:N,
  if abs(tfr(jcol,icol))>Threshold,
   jcolhat= jcol - tf2(jcol,icol);
   jcolhat=rem(rem(jcolhat-1,N)+N,N)+1;
   rtfr(jcolhat,icol)=rtfr(jcolhat,icol) + tfr(jcol,icol) ;
   tf2(jcol,icol)=jcolhat;
  else
   tf2(jcol,icol)=inf;
   rtfr(jcol,icol)=rtfr(jcol,icol) + tfr(jcol,icol) ;
  end;
 end;
end;

if trace, fprintf('\n'); end;
if (nargout==0),
 TFTBcontinue=1;
 while (TFTBcontinue==1),
  choice=menu ('Choose the representation:',...
               'stop',...
               'pseudo Wigner-Ville distribution',...
               'reassigned pseudo Wigner-Ville distribution');
  if (choice==1), TFTBcontinue=0;
  elseif (choice==2),
   tfrqview(tfr,x,t,'tfrpwv',h);
  elseif (choice==3),
   tfrqview(rtfr,x,t,'tfrrpwv',h);
  end;
 end;
elseif (nargout>2),
 hat=tf2;
end;


function c = upc_check_digit ( p, l, r )

%*****************************************************************************80
%
%% UPC_CHECK_DIGIT returns the check digit of a UPC.
%
%  Discussion:
%
%    UPC stands for Universal Price Code.
%
%    A full UPC is a string of 12 digits, in groups of size 1, 5, 5, and 1,
%    of the form P-LLLLL-RRRRR-C, where:
%
%      P is the one-digit product type code.
%      L is the five-digit manufacturer code.
%      R is the five_digit product code
%      C is the check digit.
%
%  Example:
%
%    0-72890-00011-8
%    0-12345-67890-5
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the one-digit product type code.
%
%    Input, integer L, the five-digit manufacturer code.
%
%    Input, integer R, the five-digit product code.
%
%    Output, integer C, the check digit.
%
  if ( p < 0 | 9 < p )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'UPC_CHECK_DIGIT - Fatal error!\n' );
    fprintf ( 1, '  P < 0 or 9 < P!\n' );
    error ( 'UPC_CHECK_DIGIT - Fatal error!' );
  end

  if ( l < 0 | 99999 < l )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'UPC_CHECK_DIGIT - Fatal error!\n' );
    fprintf ( 1, '  L < 0 or 99999 < L!\n' );
    error ( 'UPC_CHECK_DIGIT - Fatal error!' );
  end

  if ( r < 0 | 99999 < r )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'UPC_CHECK_DIGIT - Fatal error!\n' );
    fprintf ( 1, '  R < 0 or 99999 < R!\n' );
    error ( 'UPC_CHECK_DIGIT - Fatal error!' );
  end

  lc = i4_to_digits_decimal ( l, 5 );
  rc = i4_to_digits_decimal ( r, 5 );

  c = ( p + lc(2) + lc(4) + rc(1) + rc(3) + rc(5) ) * 3 ...
          + lc(1) + lc(3) + lc(5) + rc(2) + rc(4);

  c = mod ( c, 10 );

  c = mod ( 10 - c, 10 );

  return
end


function phiFaceAverage = upwindMean2D(phi, u)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the upwind average on
% the cell faces, based on the direction of the velocity vector for a uniform mesh.
%
% SYNOPSIS:
%   phiFaceAverage = upwindMean2D(phi, u)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;
uy = u.yvalue;

% check the size of the variable and the mesh dimension
Nxy = phi.domain.dims;
Nx = Nxy(1); Ny = Nxy(2);

% assign to a temp variable for boundary corrections
phi_tmp = phi.value;

% correct the value of phi at the boundary (calculation trick)
% assign the value of the left boundary to the left ghost cells
phi_tmp(1,:) = (phi.value(1,:)+phi.value(2,:))/2;
% assign the value of the right boundary to the right ghost cells
phi_tmp(end,:) = (phi.value(end,:)+phi.value(end-1,:))/2;
% assign the value of the bottom boundary to the bottom ghost cells
phi_tmp(:,1) = (phi.value(:,1)+phi.value(:,2))/2;
% assign the value of the top boundary to the top ghost cells
phi_tmp(:,end) = (phi.value(:,end)+phi.value(:,end-1))/2;

% calculate the average value
xvalue = (ux>0).*phi_tmp(1:Nx+1,2:Ny+1)+ ...
                        (ux<0).*phi_tmp(2:Nx+2,2:Ny+1)+ ...
                        0.5*(ux==0).*(phi.value(1:Nx+1,2:Ny+1)+phi.value(2:Nx+2,2:Ny+1));
yvalue = (uy>0).*phi_tmp(2:Nx+1,1:Ny+1)+ ...
                        (uy<0).*phi_tmp(2:Nx+1,2:Ny+2)+ ...
                        0.5*(uy==0).*(phi.value(2:Nx+1,1:Ny+1)+phi.value(2:Nx+1,2:Ny+2));
phiFaceAverage=FaceVariable(phi.domain, xvalue, yvalue, []);


function fem1d_bvp_quadratic_test04 ( )

%*****************************************************************************80
%
%% FEM1D_BVP_QUADRATIC_TEST04 carries out test case #4.
%
%  Discussion:
%
%    Use A4, C4, F4, EXACT4, EXACT_UX4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dianne O'Leary,
%    Scientific Computing with Case Studies,
%    SIAM, 2008,
%    ISBN13: 978-0-898716-66-5,
%    LC: QA401.O44.
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_BVP_QUADRATIC_TEST04\n' );
  fprintf ( 1, '  Solve -( A(x) U''(x) )'' + C(x) U(x) = F(x)\n' );
  fprintf ( 1, '  for 0 < x < 1, with U(0) = U(1) = 0.\n' );
  fprintf ( 1, '  A4(X)  = 1.0 + X * X\n' );
  fprintf ( 1, '  C4(X)  = 0.0\n' );
  fprintf ( 1, '  F4(X)  = ( X + 3 X^2 + 5 X^3 + X^4 ) * exp ( X )\n' );
  fprintf ( 1, '  U4(X)  = X * ( 1 - X ) * exp ( X )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', n );
%
%  Geometry definitions.
%
  x_first = 0.0;
  x_last = 1.0;
  x = linspace ( x_first, x_last, n );
  x = x(:);

  u = fem1d_bvp_quadratic ( n, @a4, @c4, @f4, x );

  uexact = exact4 ( x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    X         U         Uexact    Error\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %4d  %8f  %8f  %8f  %8e\n', ...
      i, x(i), u(i), uexact(i), abs ( u(i) - uexact(i) ) );
  end

  e1 = l1_error ( n, x, u, @exact4 );
  e2 = l2_error_quadratic ( n, x, u, @exact4 );
  h1s = h1s_error_quadratic ( n, x, u, @exact_ux4 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 norm of error  = %g\n', e1 );
  fprintf ( 1, '  L2 norm of error  = %g\n', e2 );
  fprintf ( 1, '  Seminorm of error = %g\n', h1s );

  return
end
function value = a4 ( x )

%*****************************************************************************80
%
%% A4 evaluates A function #4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0 + x .* x;

  return
end
function value = c4 ( x )

%*****************************************************************************80
%
%% C4 evaluates C function #4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 0.0;

  return
end
function value = exact4 ( x )

%*****************************************************************************80
%
%% EXACT4 evaluates exact solution #4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of U(X).
%
  value = x .* ( 1.0 - x ) .* exp ( x );

  return
end
function value = exact_ux4 ( x )

%*****************************************************************************80
%
%% EXACT_UX4 evaluates the derivative of exact solution #4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of dUdX(X).
%
  value = ( 1.0 - x - x .* x ) .* exp ( x );

  return
end
function value = f4 ( x )

%*****************************************************************************80
%
%% F4 evaluates right hand side function #4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = ( x + 3.0 * x.^2 + 5.0 * x.^3 + x.^4 ) .* exp ( x );

  return
end


function b = r83_to_r8ge ( n, a )

%*****************************************************************************80
%
%% R83_TO_R8GE copies an R83 matrix to a R8GE matrix.
%
%  Discussion:
%
%    The R83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 2.
%
%    Input, real A(3,N), the R83 matrix.
%
%    Output, real B(N,N), the R8GE matrix.
%
  for i = 1 : n
    for j = 1 : n

      if ( j == i-1 )
        b(i,j) = a(3,j);
      elseif ( j == i )
        b(i,j) = a(2,j);
      elseif ( j == i+1 )
        b(i,j) = a(1,j);
      else
        b(i,j) = 0.0;
      end

    end
  end

  return
end


function errplotl(sol,eldata,ev,xy,x,y,fig)
%errplotl   plots solution and error estimate on L-shaped domain
%   errplotl(sol,eldata,ev,xy,x,y,fig)
%   input
%          sol        nodal solution vector
%          eldata     element error vector
%          ev         element mapping matrix
%          xy         vertex coordinate vector
%          x          vector of x-axis interpolation points
%          y          vector of y-axis interpolation points
%          fig        figure number
%
%   IFISS function: DJS; 5 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
fprintf('plotting solution and estimated errors... ')
% interpolate to a cartesian product mesh
[X,Y]=meshgrid(x,y);
xysol = griddata(xy(:,1),xy(:,2),sol,X,Y);
[II,JJ]=find(X<0 & Y<0); xysol(II,JJ)=nan;
figure(fig)
subplot(221),contour(X,Y,xysol,20),axis('square')
axis('off'), ellx, title('Finite Element Solution')
subplot(222),mesh(X,Y,xysol),axis('square')
view(330,30)
%%
xx=xy(:,1); yy=xy(:,2);
nel=length(eldata);
% loop over elements
for ielem = 1:nel
xl = xx(ev(ielem,:));
yl = yy(ev(ielem,:));
xc(ielem,1) = 0.25*sum(xl);
xc(ielem,2) = 0.25*sum(yl);
end
%
% interpolate to a cartesian product mesh
x=0.5*(x(1:end-1)+x(2:end));
y=0.5*(y(1:end-1)+y(2:end));
[X,Y]=meshgrid(x,y);
xysol = griddata(xc(:,1),xc(:,2),eldata,X,Y);
[II,JJ]=find(X<0 & Y<0); xysol(II,JJ)=nan;
subplot(223),contour(X,Y,xysol,15),axis('square')
axis('off'), ellx, title('Estimated Error')
subplot(224),mesh(X,Y,xysol),axis('square')
view(330,30)
subplot(111)
fprintf('done\n')
return


classdef MOEADDE_F4 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing MOEA/D-DE

%------------------------------- Reference --------------------------------
% H. Li and Q. Zhang, Multiobjective optimization problems with complicated
% Pareto sets, MOEA/D and NSGA-II, IEEE Transactions on Evolutionary
% Computation, 2009, 13(2): 284-302.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = [0,-ones(1,obj.D-1)];
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            J1 = 3 : 2 : obj.D;
            J2 = 2 : 2 : obj.D;
            PopObj(:,1) = X(:,1)         + 2*mean((X(:,J1)-0.8*repmat(X(:,1),1,length(J1)).*cos(repmat(2*pi*X(:,1),1,length(J1))+repmat(J1*pi/obj.D/3,size(X,1),1))).^2,2);
            PopObj(:,2) = 1-sqrt(X(:,1)) + 2*mean((X(:,J2)-0.8*repmat(X(:,1),1,length(J2)).*sin(repmat(6*pi*X(:,1),1,length(J2))+repmat(J2*pi/obj.D,size(X,1),1))).^2,2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^0.5;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function linpack_s_test07 ( )

%*****************************************************************************80
%
%% TEST07 tests SGBFA and SGBSL.
%
%  Discussion:
%
%    SGBFA and SGBSL are for general banded matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;
  ml = 25;
  mu = 25;
  lda = 2*ml+mu+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST07\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  SGBFA factors the matrix,\n' );
  fprintf ( 1, '  SGBSL solves a factored linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Assign values to matrix A and right hand side B.
%
%  We want to try a problem with a significant bandwidth.
%
  m = ml + mu + 1;
  fprintf ( 1, '  The bandwidth of the matrix is %d\n', m );

  for j = 1 : n

    ilo = max ( 1, j - mu );
    ihi = min ( n, j + ml );

    temp = 0.0;
    for i = ilo : ihi
      a(i-j+m,j) = -1.0;
      temp = temp - 1.0;
    end

    temp = temp + 1.0;
    a(m,j) = 4.0 - temp;
    b(j) = 4.0;

  end
%
%  Force B to be a column vector.
%
  b = b';
%
%  Factor the matrix A.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, ipivot, info ] = sgbfa ( a, lda, n, ml, mu );

  if ( info ~= 0 )
   fprintf ( 1, '  Error!  SGBFA returns INFO = %d\n', info );
    return
  end
%
%  Call SGBSL to solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  job = 0;
  b = sgbsl ( a, lda, n, ml, mu, ipivot, b, job );
%
%  Print the results.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last 5 entries of the solution:\n' );
  fprintf ( 1, '  (All should be 1):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end