text:
function [fid, tree, dir] = fiff_open_le(fname)
%
% [fid, tree, dir] = fiff_open_le(fname)
%
% Open a fif file and provide the directory of tags
%
% fid     the opened file id
% tree    tag directory organized into a tree
% dir     the sequential tag directory
%
% This is a modified version, specific for opening 'little endian' fiff files
% Arjen Stolk

%
%   Author : Matti Hamalainen, MGH Martinos Center
%   License : BSD 3-clause
%
%   Revision 1.7  2009/03/30 11:37:37  msh
%   Added copying of measurement info blocks from the original like in mne_browse_raw
%
%   Revision 1.6  2008/11/16 21:31:23  msh
%   Added mne_transform_coordinates and new coordinate frame definitions
%
%   Revision 1.5  2006/05/03 19:03:19  msh
%   Eliminated the use of cast function for Matlab 6.5 compatibility
%
%   Revision 1.4  2006/04/23 15:29:40  msh
%   Added MGH to the copyright
%
%   Revision 1.3  2006/04/18 20:44:46  msh
%   Added reading of forward solution.
%   Use length instead of size when appropriate
%
%   Revision 1.2  2006/04/17 15:01:34  msh
%   More small improvements.
%
%   Revision 1.1  2006/04/10 23:26:54  msh
%   Added fiff reading routines
%

global FIFF;
if isempty(FIFF)
    FIFF = fiff_define_constants();
end

me='MNE:fiff_open';
verbose=false;
fid = fopen_or_error(fname,'rb','ieee-le'); % Arjen Stolk: this is 'ieee-be' in fiff_open.m

%
%   Check that this looks like a fif file
%
tag = fiff_read_tag_info(fid);
if tag.kind ~= FIFF.FIFF_FILE_ID
    ft_error(me,'file does not start with a file id tag');
end
if tag.type ~= FIFF.FIFFT_ID_STRUCT
    ft_error(me,'file does not start with a file id tag');
end
if tag.size ~= 20
    ft_error(me,'file does not start with a file id tag');
end
tag = fiff_read_tag(fid);
if tag.kind ~= FIFF.FIFF_DIR_POINTER
    ft_error(me,'file does have a directory pointer');
end
if nargout == 1
    fseek(fid,0,'bof');
    return;
end
%
%   Read or create the directory tree
%
if verbose
    fprintf(1,'\tCreating tag directory for %s...',fname);
end
dirpos = double(tag.data);
if dirpos > 0
    tag = fiff_read_tag(fid,dirpos);
    dir = tag.data;
else
    k = 0;
    fseek(fid,0,'bof');
    dir = struct('kind',{},'type',{},'size',{},'pos',{});
    while tag.next >= 0
        pos = ftell(fid);
        tag = fiff_read_tag_info(fid);
        k = k + 1;
        dir(k).kind = tag.kind;
        dir(k).type = tag.type;
        dir(k).size = tag.size;
        dir(k).pos  = pos;
    end
end
%
%   Create the directory tree structure
%
tree = fiff_make_dir_tree(fid,dir);
if verbose
    fprintf(1,'[done]\n');
end
%
%   Back to the beginning
%
fseek(fid,0,'bof');
return;

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
function I2 = lensdistort(I, k, varargin)
%LENSDISTORT corrects for barrel and pincusion lens abberations
%   I = LENSDISTORT(I, k)corrects for radially symmetric distortions, where
%   I is the input image and k is the distortion parameter. lens distortion
%   can be one of two types: barrel distortion and pincushion distortion.
%   In "barrel distortion", image magnification decreases with
%   distance from the optical axis. The apparent effect is that of an image
%   which has been mapped around a sphere (or barrel). In "pincushion
%   distortion", image magnification increases with the distance from the
%   optical axis. The visible effect is that lines that do not go through the
%   centre of the image are bowed inwards, towards the centre of the image,
%   like a pincushion [1].
%
%   I = LENSDISTORT(...,PARAM1,VAL1,PARAM2,VAL2,...) creates a new image image,
%   specifying parameters and corresponding values that control various aspects
%   of the image distortion correction. Parameter names case does not matter.
%
%   Parameters include:
%
%   'bordertype'            String that controls the treatment of the image
%                           edges. Valid strings are 'fit' and 'crop'. By
%                           default, 'bordertype' is set to 'crop'.
%
%   'interpolation'         String that specifies the interpolating kernel
%                           that the separable resampler uses. Valid
%                           strings are 'cubic', 'linear' and 'nearest'. By
%                           default, the 'interpolation' is set to 'cubic'
%
%   'padmethod'             string that controls how the resampler
%                           interpolates or assigns values to output elements
%                           that map close to or outside the edge of the input
%                           array. Valid strings are 'bound', circular',
%                           'fill', 'replicate', and symmetric'. By
%                           default, the 'padmethod' is set to 'fill'
%
%   'ftype'                 Integer between 1 and 4 that specifies the
%                           distortion model to be used. The models
%                           available are
%
%                           'ftype' = 1:    s = r.*(1./(1+k.*r));
%
%                           'ftype' = 2:    s = r.*(1./(1+k.*(r.^2)));
%
%                           'ftype' = 3:    s = r.*(1+k.*r);
%
%                           'ftype' = 4:    s = r.*(1+k.*(r.^2));
%
%                           By default, the 'ftype' is set to 4.
%
%   Class Support
%   -------------
%   An input intensity image can be uint8, int8, uint16, int16, uint32,
%   int32, single, double, or logical. An input indexed image can be uint8,
%   uint16, single, double, or logical.
%
%   Examples
%   --------
%       % read image
%       I = imread('cameraman.tif');
%
%       % Distort Image
%       I2 = lensdistort(I, 0.1);
%
%       % Display both images
%       imshow(I), figure, imshow(I2)
%
%   References
%   --------------
%   [1] http://en.wikipedia.org/wiki/Distortion_(optics), August 2012.
%
%   [2] Harri Ojanen, "Automatic Correction of Lens Distortion by Using
%       Digital Image Processing," July 10, 1999.
%
%   [3] G.Vassy and T.Perlaki, "Applying and removing lens distortion in post
%       production," year???
%
%   [4] http://www.mathworks.com/products/demos/image/...
%       create_gallery/tform.html#34594, August 2012.
%
%   Created by Jaap de Vries, 8/31/2012
%   jpdvrs@yahoo.com
%
%-----------------------------------------------------------------------%

%-------------------------------------------------------------------------
% This part of the codes creates variable input parameters using the input
% parser object
p = inputParser;
%   Make input string case independant
p.CaseSensitive = false;

%   Specifies the required inputs
addRequired(p,'I',@isnumeric);
addRequired(p,'k',@isnumeric);

%   Sets the default values for the optional parameters
defaultFtype = 4;
defaultBorder = 'crop';
defaultInterpolation = 'cubic';
defaultPadmethod = 'fill';

%   Specifies valid strings for the optional parameters
validBorder = {'fit','crop'};
validInterpolation = {'cubic','linear', 'nearest'};
validPadmethod = {'bound','circular', 'fill', 'replicate', 'symmetric'};

%   Funtion handles to determine wheter a proper input string has been used
checkBorder = @(x) any(validatestring(x,validBorder));
checkInterpolation = @(x) any(validatestring(x,validInterpolation));
checkPadmethod = @(x) any(validatestring(x,validPadmethod));

%   Create optional inputs
addParamValue(p,'bordertype',defaultBorder,checkBorder);
addParamValue(p,'interpolation',defaultInterpolation,checkInterpolation);
addParamValue(p,'padmethod',defaultPadmethod,checkPadmethod);
addParamValue(p,'ftype',defaultFtype,@isnumeric);

%   Pass all parameters and input to the parse method
parse(p,I,k,varargin{:});

%-------------------------------------------------------------------------
% This determines wether its a color (M,N,3) or gray scale (M,N,1) image
if ndims(I) == 3
     for i=1:3
        I2(:,:,i) = imdistcorrect(I(:,:,i),k);
     end
elseif ismatrix(I)
    I2 = imdistcorrect(I,k);
else
    error('Unknown image dimensions')
end

%-------------------------------------------------------------------------
% Nested function that perfoms the transformation
    function I3 = imdistcorrect(I,k)
    % Determine the size of the image to be distorted
    [M N]=size(I);
    center = [round(N/2) round(M/2)];
    % Creates N x M (#pixels) x-y points
    [xi,yi] = meshgrid(1:N,1:M);
    % Creates converst the mesh into a colum vector of coordiantes relative to
    % the center
    xt = xi(:) - center(1);
    yt = yi(:) - center(2);
    % Converts the x-y coordinates to polar coordinates
    [theta,r] = cart2pol(xt,yt);
    % Calculate the maximum vector (image center to image corner) to be used
    % for normalization
    R = sqrt(center(1)^2 + center(2)^2);
    % Normalize the polar coordinate r to range between 0 and 1
    r = r/R;
    % Aply the r-based transformation
    s = distortfun(r,k,p.Results.ftype);
    % un-normalize s
    s2 = s * R;
    % Find a scaling parameter based on selected border type
    brcor = bordercorrect(r,s,k, center, R);

    s2 = s2 * brcor;

    % Convert back to cartesian coordinates
    [ut,vt] = pol2cart(theta,s2);

    u = reshape(ut,size(xi)) + center(1);
    v = reshape(vt,size(yi)) + center(2);
    tmap_B = cat(3,u,v);
    resamp = makeresampler(p.Results.interpolation, p.Results.padmethod);
    I3 = tformarray(I,[],resamp,[2 1],[1 2],[],tmap_B,255);
    end

%-------------------------------------------------------------------------
% Nested function that creates a scaling parameter based on the
% 'bordertype' selected
    function x = bordercorrect(r,s,k,center, R)
        if k < 0
            if strcmp(p.Results.bordertype, 'fit')
               x = r(1)/s(1);
            end
            if strcmp(p.Results.bordertype,'crop')
               x = 1/(1 + k*(min(center)/R)^2);
            end
        elseif k > 0
            if strcmp(p.Results.bordertype, 'fit')
               x = 1/(1 + k*(min(center)/R)^2);
            end
            if strcmp(p.Results.bordertype, 'crop')
               x = r(1)/s(1);
            end
        end
    end

%-------------------------------------------------------------------------
% Nested function that pics the model type to be used
    function s = distortfun(r,k,fcnum)
        switch fcnum
        case(1)
            s = r.*(1./(1+k.*r));
        case(2)
            s = r.*(1./(1+k.*(r.^2)));
        case(3)
            s = r.*(1+k.*r);
        case(4)
            s = r.*(1+k.*(r.^2));
        end
    end

end

author: Sable

repo: mcbench-benchmarks


text:
function sub_sst = delete_sst(sst,t1,t2,edge_mode)

%DELETE_SST: Delete event SST (Start/Stop Times) between t1 and t2
%
%USAGE: sub_sst = delete_sst(sst,t1,t2,edge_mode)
%
%INPUTS: sst - Event Start/Stop Times [Nx2 array of numeric time values]
%        t1  - time value, delete all events in sst between t1 (start)
%              and t2 (end)
%        t2  - see 't1'
%        edge_mode  - If t1 or t2 falls between an event start time and
%                stop time in sst, edge_mode determines if the event is
%                deleted, retained, or partially deleted at time t1 or t2.
%           'delete' - whole event is removed from array sub_sst
%           'keep'   - whole event is retained in array sub_sst
%           'part'   - event in sub_sst is split - i.e. deleted
%                       before t1 or after t2
%
%OUTPUTS: sub_sst - Subset of origianl sst (Event Start/Stop Times)
%                   [Nx2 array of numeric time values]
%
% See also ADD_SST, CHK_T, COMPARE_SST, DELETE_SST, EXTRACT_SST, IS_SST,
%          ISTEQUAL, MERGE_SST, SEARCH_SST, SORT_SST, NAN2SST, SSD2SST,
%          SST2NAN, SST2SSD, SST2VAL, SST2WFA, WFA2SST
%
% Author: Dane Ketner, Alaska Volcano Observatory
% $Date$
% $Revision$

if nargin < 4
   error('DELETE_SST: Too few input arguments')
elseif nargin > 4
   error('DELETE_SST: Too many input arguments')
end

if is_sst(sst)
   if iscell(sst)
      for m = 1:numel(sst)
         sub_sst{m} = DELETE_SST(sst{m},t1,t2,edge_mode);
      end
   else
      sub_sst = DELETE_SST(sst,t1,t2,edge_mode);
   end
else
   error('DELETE_SST: Not a valid Start/Stop Time Argument')
end

%%
function sub_sst = DELETE_SST(sst,t1,t2,edge_mode)

[N1 P1] = search_sst(t1,sst);
[N2 P2] = search_sst(t2,sst);

% 'first' and 'last' refer to the first and last events within the time
%  span defined by t1 and t2. N1 is the event number within sst
%  corresponding to t1, and P1 is the corresponding event position with
%  relation to t1. P1 = 1 if t1 falls inside the event time. P1 = 0 if t1
%  is before the start of the event.

if P1 == 1
   if strcmpi(edge_mode,'part')
      first = [sst(N1,1) t1];
   elseif strcmpi(edge_mode,'delete')
      first = [];
   elseif strcmpi(edge_mode,'keep')
      first = sst(N1,:);
   end
elseif P1 == 0
   first = [];
end

if P2 == 1
   if strcmpi(edge_mode,'part')
      last = [t2 sst(N2,2)];
   elseif strcmpi(edge_mode,'delete')
      last = [];
   elseif strcmpi(edge_mode,'keep')
      last = sst(N2,:);
   end
elseif P2 == 0
   last = sst(N2,:);
end

sub_sst = [sst(1:N1-1,:); first; last; sst(N2+1:end,:)];

author: geoscience-community-codes

repo: GISMO


text:
function u=dimsplit(fflux,gflux,u0,x,y,T,nstep)
%%
%  Solves the equation u_t + fflux(u)_x  + gflux(u)_y = 0 by operator
%  splitting.  u0 and a must the size of x.
%
%  Output is a matrix of size [length(x),((T/dt)+1)] .
%

%% Initial setup
h=path;
path(h,'../Example2_5');
Nt=nstep;
dt=T/Nt;
dx=x(2)-x(1); m=length(x);
dy=y(2)-y(1); n=length(y);
u=zeros(m,n,Nt+1);
u(:,:,1)=u0;

%% Dimensional splitting
%  uses the LxF method in each direction.
for i=1:Nt,
	u1=LxF(fflux,u(:,:,i),dt,dx,1,'periodic');      % Conservation in x-dir
	u(:,:,i+1)=LxF(gflux,u1,dt,dy,2,'periodic');    % Conservation in y-dir
end;

path(h);

repo: Aero-matlab


text:
function varargout = subsref(F, index)
%SUBSREF   BALLFUNV subsref.
%
% ( )
%   F(X,Y,Z) returns the values of the BALLFUNV F evaluated on the array
%   (X,Y,Z) in cartesian coordinates.
%
%   F(R, L, TH, 'spherical') returns the values of the BALLFUNV F evaluated
%   on the array (R,TH,LAM) in spherical coordinates.
%
%   F(K) returns the first component of F if K = 1, the second if K = 2, and
%   the third if K = 3.
%
%   F(R, :, :, 'spherical') returns a SPHEREFUNV representing the BALLFUNV
%   F along a radial shell.
%
%   F(:, :, :) returns F.
%
%  .
%   F.PROP returns the property PROP of F as defined by GET(F,'PROP').
%
% { }
%    Throws an error.

% Copyright 2019 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% check for empty CHEBFUN2V object.
if ( isempty(F) )
    varargout = {[]};
    return
end

% Recursive going through the ref structure:
idx = index(1).subs;
switch index(1).type

    case '.'
        % Call GET() for .PROP access.
        out = get(F, idx);
        if ( numel(index) > 1 )
            % Recurse on SUBSREF():
            index(1) = [];
            out = subsref(out, index);
        end
        varargout = {out};

    case '()'
        if ( isa(idx{1}, 'double') && numel(idx)==1 )
            Fc = F.comp;
                if all( idx{1} == 1  )
                    varargout = {Fc{1}};
                elseif ( all( idx{1} == 2 ) )
                    varargout = {Fc{2}};
                elseif ( ( all(idx{1} == 3) ) )
                    varargout = {Fc{3}};
                else
                    error('CHEBFUN:BALLFUNV:subsref:index', ...
                        'BALLFUNV only contains three components');
                end
        else
            % F(X,Y,Z), F(X,Y,Z,str), etc.
           Fc = F.comp;
           v1 = subsref(Fc{1}, index);
           v2 = subsref(Fc{2}, index);
           v3 = subsref(Fc{3}, index);
           varargout = {[v1 ; v2 ; v3]};
        end

    otherwise
        error('CHEBFUN:BALLFUNV:subsref:unexpectedType', ...
            ['??? Unexpected index.type of ' index(1).type]);

end

% Recurse down:
if ( numel(index) > 1 )
    index(1) = [];
    varargout = { subsref( varargout{ : }, index ) };
end

end

author: chebfun

repo: chebfun

save_path: github-repos/MATLAB/chebfun-chebfun


text:
%
% Usage:   alpha=mexSOMP(X,D,list_groups,param);
%
% Name: mexSOMP
%     (this function has not been intensively tested).
%
% Description: mexSOMP is an efficient implementation of a
%     Simultaneous Orthogonal Matching Pursuit algorithm. It is optimized
%     for solving a large number of small or medium-sized
%     decomposition problem (and not for a single large one).
%     It first computes the Gram matrix D'D and then perform
%     a Cholesky-based OMP of the input signals in parallel.
%     It aims at addressing the following NP-hard problem
%
%     X is a matrix structured in groups of signals, which we denote
%     by X=[X_1,...,X_n]
%
%     for all matrices X_i of X,
%         min_{A_i} ||A_i||_{0,infty}  s.t  ||X_i-D A_i||_2^2 <= eps*n_i
%         where n_i is the number of columns of X_i
%
%         or
%
%         min_{A_i} ||X_i-D A_i||_2^2  s.t. ||A_i||_{0,infty} <= L
%
%
% Inputs: X:  double m x N matrix   (input signals)
%            m is the signal size
%            N is the total number of signals
%         D:  double m x p matrix   (dictionary)
%            p is the number of elements in the dictionary
%            All the columns of D should have unit-norm !
%         list_groups : int32 vector containing the indices (starting at 0)
%            of the first elements of each groups.
%         param: struct
%            param.L (maximum number of elements in each decomposition)
%            param.eps (threshold on the squared l2-norm of the residual
%            param.numThreads (optional, number of threads for exploiting
%            multi-core / multi-cpus. By default, it takes the value -1,
%            which automatically selects all the available CPUs/cores).
%
% Output: alpha: double sparse p x N matrix (output coefficients)
%
% Note: this function admits a few experimental usages, which have not
%     been extensively tested:
%      - single precision setting (even though the output alpha is double
%        precision)
%
% Author: Julien Mairal, 2010

author: tiepvupsu

repo: DICTOL

save_path: github-repos/MATLAB/tiepvupsu-DICTOL


text:
function Derive_Equations()
%%%% Derive Equations - Five Link Biped Model %%%%
%
% This function derives the equations of motion, as well as some other useful
% equations (kinematics, contact forces, ...) for the five-link biped
% model.
%
% This version of the code includes a few more complicated features for
% dealing with difficult cost functions. In particular, it adds 10 slack
% variables to compute the abs(power) term in the cost function, and the
% primary control is the derivative of torque, rather than torque itself.
% This allows for regularization by the derivative of the input.
%
%
% Nomenclature:
%
% - There are five links, which will be numbered starting with "1" for the
% stance leg tibia, increasing as the links are father from the base joint,
% and ending with "5" for the swing leg tibia.
%   1 - stance leg tibia (lower leg)
%   2 - stance leg femur  (upper leg)
%   3 - torso
%   4 - swing leg femur
%   5 - swing leg tibia
%
% - This script uses absolute angles, which are represented with "q". All
% angles use positive convention, with the zero angle corresponding to a
% vertically aligned link configuration. [q] = [0] has the torso balanced
% upright, with both legs fully extended straight below it.
%
% - Derivatives with respect to time are notated by prepending a "d". For
% example the rate of change in an absolute angle is "dq" and angular
% acceleration would be "ddq"
%
% - Joint positions are given with "P", center of mass positions are "G"
%

clc; clear;
disp('Creating variables and derivatives...')

%%%% Absolute orientation (angle) of each link
q1 = sym('q1', 'real');
q2 = sym('q2','real');
q3 = sym('q3','real');
q4 = sym('q4','real');
q5 = sym('q5','real');

%%%% Absolute angular rate of each link
dq1 = sym('dq1','real');
dq2 = sym('dq2','real');
dq3 = sym('dq3','real');
dq4 = sym('dq4','real');
dq5 = sym('dq5','real');

%%%% Absolute angular acceleration of each linke
ddq1 = sym('ddq1','real');
ddq2 = sym('ddq2','real');
ddq3 = sym('ddq3','real');
ddq4 = sym('ddq4','real');
ddq5 = sym('ddq5','real');

%%%% Torques at each joint
u1 = sym('u1','real');  %Stance foot
u2 = sym('u2','real');   %Stance knee
u3 = sym('u3','real');   %Stance hip
u4 = sym('u4','real');   %Swing hip
u5 = sym('u5','real');   %Swing knee

%%%% Torques rate at each joint
du1 = sym('du1','real');  %Stance foot
du2 = sym('du2','real');   %Stance knee
du3 = sym('du3','real');   %Stance hip
du4 = sym('du4','real');   %Swing hip
du5 = sym('du5','real');   %Swing knee

%%%% Slack variables -- negative component of power
sn1 = sym('sn1','real');  %Stance foot
sn2 = sym('sn2','real');   %Stance knee
sn3 = sym('sn3','real');   %Stance hip
sn4 = sym('sn4','real');   %Swing hip
sn5 = sym('sn5','real');   %Swing knee

%%%% Slack variables -- positive component of power
sp1 = sym('sp1','real');  %Stance foot
sp2 = sym('sp2','real');   %Stance knee
sp3 = sym('sp3','real');   %Stance hip
sp4 = sym('sp4','real');   %Swing hip
sp5 = sym('sp5','real');   %Swing knee

%%%% Mass of each link
m1 = sym('m1','real');
m2 = sym('m2','real');
m3 = sym('m3','real');
m4 = sym('m4','real');
m5 = sym('m5','real');

%%%% Distance between parent joint and link center of mass
c1 = sym('c1','real');
c2 = sym('c2','real');
c3 = sym('c3','real');
c4 = sym('c4','real');
c5 = sym('c5','real');

%%%% Length of each link
l1 = sym('l1','real');
l2 = sym('l2','real');
l3 = sym('l3','real');
l4 = sym('l4','real');
l5 = sym('l5','real');

%%%% Moment of inertia of each link about its own center of mass
I1 = sym('I1','real');
I2 = sym('I2','real');
I3 = sym('I3','real');
I4 = sym('I4','real');
I5 = sym('I5','real');

g = sym('g','real'); % Gravity
Fx = sym('Fx','real');   %Horizontal contact force at stance foot
Fy = sym('Fy','real');   %Vertical contact force at stance foot
empty = sym('empty','real');   %Used for vectorization, user should pass a vector of zeros
t = sym('t','real');  %dummy continuous time

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                Set up coordinate system and unit vectors                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

i = sym([1;0]);   %Horizontal axis
j = sym([0;1]);   %Vertical axis

e1 = cos(q1)*(j) + sin(q1)*(-i);  %unit vector from P0 -> P1, (contact point to stance knee)
e2 = cos(q2)*(j) + sin(q2)*(-i);  %unit vector from P1 -> P2, (stance knee to hip)
e3 = cos(q3)*(j) + sin(q3)*(-i);  %unit vector from P2 -> P3, (hip to shoulders);
e4 = -cos(q4)*(j) - sin(q4)*(-i);  %unit vector from P2 -> P4, (hip to swing knee);
e5 = -cos(q5)*(j) - sin(q5)*(-i);  %unit vector from P4 -> P5, (swing knee to swing foot);

P0 = 0*i + 0*j;   %stance foot = Contact point = origin
P1 = P0 + l1*e1;  %stance knee
P2 = P1 + l2*e2;  %hip
P3 = P2 + l3*e3;  %shoulders
P4 = P2 + l4*e4;  %swing knee
P5 = P4 + l5*e5;  %swing foot

G1 = P1 - c1*e1;  % CoM stance leg tibia
G2 = P2 - c2*e2;  % CoM stance leg febur
G3 = P3 - c3*e3;  % CoM torso
G4 = P2 + c4*e4;  % CoM swing leg femur
G5 = P4 + c5*e5;  % CoM swing leg tibia
G = (m1*G1 + m2*G2 + m3*G3 + m4*G4 + m5*G5)/(m1+m2+m3+m4+m5);  %Center of mass for entire robot

%%%% Define a function for doing '2d' cross product: dot(a x b, k)
cross2d = @(a,b)(a(1)*b(2) - a(2)*b(1));

%%%% Weight of each link:
w1 = -m1*g*j;
w2 = -m2*g*j;
w3 = -m3*g*j;
w4 = -m4*g*j;
w5 = -m5*g*j;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Derivatives                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

q = [q1;q2;q3;q4;q5];
dq = [dq1;dq2;dq3;dq4;dq5];
ddq = [ddq1;ddq2;ddq3;ddq4;ddq5];
u = [u1;u2;u3;u4;u5];
du = [du1;du2;du3;du4;du5];
sn = [sn1;sn2;sn3;sn4;sn5];
sp = [sp1;sp2;sp3;sp4;sp5];
z = [t;q;dq;u;du;sn;sp];   % time-varying vector of inputs

% Neat trick to compute derivatives using the chain rule
derivative = @(in)( jacobian(in,[q;dq;u])*[dq;ddq;du] );

% Velocity of the swing foot (used for step constraints)
dP5 = derivative(P5);

% Compute derivatives for the CoM of each link:
dG1 = derivative(G1);  ddG1 = derivative(dG1);
dG2 = derivative(G2);  ddG2 = derivative(dG2);
dG3 = derivative(G3);  ddG3 = derivative(dG3);
dG4 = derivative(G4);  ddG4 = derivative(dG4);
dG5 = derivative(G5);  ddG5 = derivative(dG5);
dG = derivative(G);  ddG = derivative(dG);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Calculations:                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

singleStanceDynamics();
objectiveFunctions();
heelStrikeDynamics();

mechanicalEnergy();
contactForces();
kinematics();

disp('Done!');

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

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                      Single-Stance Dynamics                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% I solve the dynamics here by carefully selecting angular momentum balance
% equations about each joint, working my way out the kinematic tree from
% the root.

    function singleStanceDynamics()
        disp('Deriving single stance dynamics...')

        %%%% AMB - entire system @ P0
        eqnTorque0 = ...
            cross2d(G1-P0,w1) + ...
            cross2d(G2-P0,w2) + ...
            cross2d(G3-P0,w3) + ...
            cross2d(G4-P0,w4) + ...
            cross2d(G5-P0,w5) + ...
            u1;

        eqnInertia0 = ...
            cross2d(G1-P0,m1*ddG1) + ddq1*I1 + ...
            cross2d(G2-P0,m2*ddG2) + ddq2*I2 + ...
            cross2d(G3-P0,m3*ddG3) + ddq3*I3 + ...
            cross2d(G4-P0,m4*ddG4) + ddq4*I4 + ...
            cross2d(G5-P0,m5*ddG5) + ddq5*I5;

        %%%% AMB - swing leg, torso, stance femer  @ stance knee
        eqnTorque1 = ...
            cross2d(G2-P1,w2) + ...
            cross2d(G3-P1,w3) + ...
            cross2d(G4-P1,w4) + ...
            cross2d(G5-P1,w5) + ...
            u2;

        eqnInertia1 = ...
            cross2d(G2-P1,m2*ddG2) + ddq2*I2  + ...
            cross2d(G3-P1,m3*ddG3) + ddq3*I3  + ...
            cross2d(G4-P1,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P1,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing leg, torso @ hip
        eqnTorque2 = ...
            cross2d(G3-P2,w3) + ...
            cross2d(G4-P2,w4) + ...
            cross2d(G5-P2,w5) + ...
            u3;

        eqnInertia2 = ...
            cross2d(G3-P2,m3*ddG3) + ddq3*I3  + ...
            cross2d(G4-P2,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P2,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing leg @ hip
        eqnTorque3 = ...
            cross2d(G4-P2,w4) + ...
            cross2d(G5-P2,w5) + ...
            u4;

        eqnInertia3 = ...
            cross2d(G4-P2,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P2,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing tibia % swing knee
        eqnTorque4 = ...
            cross2d(G5-P4,w5) + ...
            u5;

        eqnInertia4 = ...
            cross2d(G5-P4,m5*ddG5) + ddq5*I5 ;

        %%%% Collect and solve equations:
        eqns = [...
            eqnTorque0 - eqnInertia0;
            eqnTorque1 - eqnInertia1;
            eqnTorque2 - eqnInertia2;
            eqnTorque3 - eqnInertia3;
            eqnTorque4 - eqnInertia4];

        [MM, FF] = equationsToMatrix(eqns,ddq);  % ddq = MM\ff;

        %%%% Compute gradients:
        [m, mi, mz, mzi, mzd] = computeGradients(MM,z,empty);
        [f, fi, fz, fzi, fzd] = computeGradients(FF,z,empty);

        % Write function file:
        matlabFunction(m, mi, f, fi,...   %dynamics
            mz, mzi, mzd, fz, fzi, fzd,...  %gradients
            'file','autoGen_dynSs.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'u1','u2','u3','u4','u5',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g','empty'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Objective Functions                              %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function objectiveFunctions()

        % Joint rates:
        v1 = dq1;   % joint rate 1
        v2 = dq2-dq1;   % joint rate 2
        v3 = dq3-dq2; % joint rate 3
        v4 = dq4-dq3;  % joint rate 4
        v5 = dq5-dq4;  % joint rate 5

        % Compute the power used by each joint
        pow1 = v1*u1;  %Power used by joint 1
        pow2 = v2*u2;  %Power used by joint 2
        pow3 = v3*u3;  %Power used by joint 3
        pow4 = v4*u4;  %Power used by joint 4
        pow5 = v5*u5;  %Power used by joint 5

        % Constraint on the slack variables:
        slackCst = [...
            pow1 - (sp1 - sn1);
            pow2 - (sp2 - sn2);
            pow3 - (sp3 - sn3);
            pow4 - (sp4 - sn4);
            pow5 - (sp5 - sn5)];

        % Gradients of the constraint on slack variables:
        [c, ~, cz, czi, ~] = computeGradients(slackCst,z,empty);

        matlabFunction(c,cz,czi,...
            'file','autoGen_cst_costOfTransport.m',...
            'vars',{...
            'dq1','dq2','dq3','dq4','dq5',...
            'u1','u2','u3','u4','u5',...
            'sn1','sn2','sn3','sn4','sn5',...
            'sp1','sp2','sp3','sp4','sp5','empty'});

        % abs(power) using slack variables:
        gammaNeg = sym('gammaNeg','real');  %Torque-squared smoothing parameter
        gammaPos = sym('gammaPos','real');  %Torque-squared smoothing parameter
        absPower = gammaNeg*(sn1 + sn2 + sn3 + sn4 + sn5) + ...
            gammaPos*(sp1 + sp2 + sp3 + sp4 + sp5);

        % Cost of Transport:
        weight = (m1+m2+m3+m4+m5)*g;
        stepLength = sym('stepLength','real');
        alpha = sym('alpha','real');  %Torque-squared smoothing parameter
        beta = sym('beta','real');  %Torque-rate squared smoothing
        F = absPower/(weight*stepLength) + ...
            alpha*(u1^2 + u2^2 + u3^2 + u4^2 + u5^2) + ...
            beta*(du1^2 + du2^2 + du3^2 + du4^2 + du5^2);
        [f, ~, fz, fzi, ~]  = computeGradients(F,z,empty);

        matlabFunction(f,fz,fzi,...
            'file','autoGen_obj_costOfTransport.m',...
            'vars',{...
            'm1','m2','m3','m4','m5',...
            'u1','u2','u3','u4','u5',...
            'du1','du2','du3','du4','du5',...
            'sn1','sn2','sn3','sn4','sn5', ...
            'sp1','sp2','sp3','sp4','sp5',...
            'g','stepLength','gammaNeg','gammaPos','alpha','beta','empty'});

        % Swing foot height:
        stepHeight = sym('stepHeight','real');
        yFoot = P5(2);
        xFoot = P5(1);
        yMin = stepHeight*(1 - (xFoot/stepLength)^2);
        yCst = yMin - yFoot;  %Must be negative
        [y, ~, yz, yzi, ~]  = computeGradients(yCst,z,empty);
        matlabFunction(y,yz,yzi,...
                    'file','autoGen_cst_swingFootHeight.m',...
            'vars',{...
            'q1','q2','q4','q5',...
            'l1','l2','l4','l5'...
            'stepLength','stepHeight'});
    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Heel-Strike Dynamics                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function heelStrikeDynamics()
        disp('Deriving heel-strike dynamics...')

        %%%% Notes:
        % xF - heelStrike(xI) --> constraint --> 0
        % xF - collision(footSwap(xI));
        %

        % Angles before heel-strike:
        q1m = sym('q1m','real');
        q2m = sym('q2m','real');
        q3m = sym('q3m','real');
        q4m = sym('q4m','real');
        q5m = sym('q5m','real');
        qm = [q1m;q2m;q3m;q4m;q5m];

        % Angles after heel-strike
        q1p = sym('q1p','real');
        q2p = sym('q2p','real');
        q3p = sym('q3p','real');
        q4p = sym('q4p','real');
        q5p = sym('q5p','real');
        qp = [q1p;q2p;q3p;q4p;q5p];

        % Angular rates before heel-strike:
        dq1m = sym('dq1m','real');
        dq2m = sym('dq2m','real');
        dq3m = sym('dq3m','real');
        dq4m = sym('dq4m','real');
        dq5m = sym('dq5m','real');
        dqm = [dq1m;dq2m;dq3m;dq4m;dq5m];

        % Angular rates after heel-strike
        dq1p = sym('dq1p','real');
        dq2p = sym('dq2p','real');
        dq3p = sym('dq3p','real');
        dq4p = sym('dq4p','real');
        dq5p = sym('dq5p','real');
        dqp = [dq1p;dq2p;dq3p;dq4p;dq5p];

        % torque before heel-strike:
        u1m = sym('u1m','real');
        u2m = sym('u2m','real');
        u3m = sym('u3m','real');
        u4m = sym('u4m','real');
        u5m = sym('u5m','real');
        um = [u1m;u2m;u3m;u4m;u5m];

        % torque after heel-strike
        u1p = sym('u1p','real');
        u2p = sym('u2p','real');
        u3p = sym('u3p','real');
        u4p = sym('u4p','real');
        u5p = sym('u5p','real');
        up = [u1p;u2p;u3p;u4p;u5p];

        % Compute kinematics before heel-strike:
        inVars = {'q1','q2','q3','q4','q5','dq1','dq2','dq3','dq4','dq5'};
        outVarsM = {'q1m','q2m','q3m','q4m','q5m','dq1m','dq2m','dq3m','dq4m','dq5m'};
        %         P0m = subs(P0,inVars,outVarsM);
        P1m = subs(P1,inVars,outVarsM);
        P2m = subs(P2,inVars,outVarsM);
        %         P3m = subs(P3,inVars,outVarsM);
        P4m = subs(P4,inVars,outVarsM);
        P5m = subs(P5,inVars,outVarsM);
        dP5m = subs(dP5,inVars,outVarsM);
        G1m = subs(G1,inVars,outVarsM);
        G2m = subs(G2,inVars,outVarsM);
        G3m = subs(G3,inVars,outVarsM);
        G4m = subs(G4,inVars,outVarsM);
        G5m = subs(G5,inVars,outVarsM);
        dG1m = subs(dG1,inVars,outVarsM);
        dG2m = subs(dG2,inVars,outVarsM);
        dG3m = subs(dG3,inVars,outVarsM);
        dG4m = subs(dG4,inVars,outVarsM);
        dG5m = subs(dG5,inVars,outVarsM);

        % Compute kinematics after heel-strike:
        outVarsP = {'q1p','q2p','q3p','q4p','q5p','dq1p','dq2p','dq3p','dq4p','dq5p'};
        P0p = subs(P0,inVars,outVarsP);
        P1p = subs(P1,inVars,outVarsP);
        P2p = subs(P2,inVars,outVarsP);
        %         P3p = subs(P3,inVars,outVarsP);
        P4p = subs(P4,inVars,outVarsP);
        %         P5p = subs(P5,inVars,outVarsP);
        dP5p = subs(dP5,inVars,outVarsP);
        G1p = subs(G1,inVars,outVarsP);
        G2p = subs(G2,inVars,outVarsP);
        G3p = subs(G3,inVars,outVarsP);
        G4p = subs(G4,inVars,outVarsP);
        G5p = subs(G5,inVars,outVarsP);
        dG1p = subs(dG1,inVars,outVarsP);
        dG2p = subs(dG2,inVars,outVarsP);
        dG3p = subs(dG3,inVars,outVarsP);
        dG4p = subs(dG4,inVars,outVarsP);
        dG5p = subs(dG5,inVars,outVarsP);

        %%%% AMB - entire system @ New stance foot
        eqnHs0m = ...   %Before collision
            cross2d(G1m-P5m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P5m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G3m-P5m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G4m-P5m,m4*dG4m) + dq4m*I4 + ...
            cross2d(G5m-P5m,m5*dG5m) + dq5m*I5;
        eqnHs0 = ...   %After collision
            cross2d(G1p-P0p,m1*dG1p) + dq1p*I1 + ...
            cross2d(G2p-P0p,m2*dG2p) + dq2p*I2 + ...
            cross2d(G3p-P0p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P0p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P0p,m5*dG5p) + dq5p*I5;

        %%%% AMB - new swing leg, torso, stance femer  @  stance knee
        eqnHs1m = ...   %Before collision
            cross2d(G1m-P4m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P4m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G3m-P4m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G4m-P4m,m4*dG4m) + dq4m*I4;
        eqnHs1 = ...   %After collision
            cross2d(G2p-P1p,m2*dG2p) + dq2p*I2 + ...
            cross2d(G3p-P1p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P1p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P1p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing leg, torso  @ new hip
        eqnHs2m = ...   %Before collision
            cross2d(G3m-P2m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G2m-P2m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G1m-P2m,m1*dG1m) + dq1m*I1;
        eqnHs2 = ...   %After collision
            cross2d(G3p-P2p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P2p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P2p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing leg @ new hip
        eqnHs3m = ...   %Before collision
            cross2d(G1m-P2m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P2m,m2*dG2m) + dq2m*I2;
        eqnHs3 = ...   %After collision
            cross2d(G4p-P2p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P2p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing tibia @ new swing knee
        eqnHs4m = ...   %Before collision
            cross2d(G1m-P1m,m1*dG1m) + dq1m*I1;
        eqnHs4 = ...   %After collision
            cross2d(G5p-P4p,m5*dG5p) + dq5p*I5;

        %%%% Collect and solve equations:
        eqnHs = [...
            eqnHs0m - eqnHs0;
            eqnHs1m - eqnHs1;
            eqnHs2m - eqnHs2;
            eqnHs3m - eqnHs3;
            eqnHs4m - eqnHs4];
        [MM, FF] = equationsToMatrix(eqnHs,dqp);

        %%%% Compute gradients:
        tp = sym('tp','real');   %Initial trajectory time
        tm = sym('tm','real');   %Final trajectory time
        zBnd = [tp;qp;dqp;up;tm;qm;dqm;um];
        [m, mi, mz, mzi, mzd] = computeGradients(MM,zBnd,empty);
        [f, fi, fz, fzi, fzd] = computeGradients(FF,zBnd,empty);

        % Heel-strike
        matlabFunction(m, mi, f, fi,...   %dynamics
            mz, mzi, mzd, fz, fzi, fzd,...  %gradients
            'file','autoGen_cst_heelStrike.m',...
            'vars',{...
            'q1p','q2p','q3p','q4p','q5p',...
            'q1m','q2m','q3m','q4m','q5m',...
            'dq1m','dq2m','dq3m','dq4m','dq5m',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4','l5',...
            'c1','c2','c3','c4','c5','empty'});

        % Collision velocity of the swing foot:
        cst = [-dP5p(2); dP5m(2)];  %Swing foot velocity before and after collision (negative sign is intentional, since output is constrained to be negative);
        cstJac = jacobian(cst,zBnd);  %Gradient
        matlabFunction(cst, cstJac,...
            'file','autoGen_cst_footVel.m',...
            'vars',{...
            'q1p','q2p','q4p','q5p',...
            'q1m','q2m','q4m','q5m',...
            'dq1p','dq2p','dq4p','dq5p',...
            'dq1m','dq2m','dq4m','dq5m',...
            'l1','l2','l4','l5'});

        % Step length and height constraint:
        stepLength = sym('stepLength','real');
        ceq = [P5m(1)-stepLength; P5m(2)];
        ceqJac = jacobian(ceq,zBnd);  %Gradient
        matlabFunction(ceq, ceqJac,...
            'file','autoGen_cst_steplength.m',...
            'vars',{...
            'q1m','q2m','q4m','q5m',...
            'l1','l2','l4','l5','stepLength'});
    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Mechanical Energy                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function mechanicalEnergy()
        disp('Deriving mechanical energy...')

        %%%% Energy:
        KineticEnergy = ...
            0.5*m1*dot(dG1,dG1) + 0.5*I1*dq1^2 + ...
            0.5*m2*dot(dG2,dG2) + 0.5*I2*dq2^2 + ...
            0.5*m3*dot(dG3,dG3) + 0.5*I3*dq3^2 + ...
            0.5*m4*dot(dG4,dG4) + 0.5*I4*dq4^2 + ...
            0.5*m5*dot(dG5,dG5) + 0.5*I5*dq5^2;
        PotentialEnergy = ...
            m1*g*G1(2) + ...
            m2*g*G2(2) + ...
            m3*g*G3(2) + ...
            m4*g*G4(2) + ...
            m5*g*G5(2);

        matlabFunction(KineticEnergy, PotentialEnergy,...
            'file','autoGen_energy.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g'},...
            'outputs',{'KE','PE'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Contact Forces                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function contactForces()

        %%%% Contact Forces:
        eqnForce5 = w1 + w2 + w3 + w4 + w5 + Fx*i + Fy*j;
        eqnInertia5 = (m1+m2+m3+m4+m5)*ddG;
        [AA,bb] = equationsToMatrix(eqnForce5-eqnInertia5,[Fx;Fy]);
        ContactForces = AA\bb;

        matlabFunction(ContactForces(1),ContactForces(2),...
            'file','autoGen_contactForce.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'ddq1','ddq2','ddq3','ddq4','ddq5',...
            'm1','m2','m3','m4','m5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g'},...
            'outputs',{'Fx','Fy'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Write Kinematics Files                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function kinematics()
        disp('Writing kinematics files...')

        P = [P1; P2; P3; P4; P5];
        Gvec = [G1; G2; G3; G4; G5];

        % Used for plotting and animation
        matlabFunction(P,Gvec,'file','autoGen_getPoints.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'l1','l2','l3','l4','l5',...
            'c1','c2','c3','c4','c5'},...
            'outputs',{'P','Gvec'});

    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Helper Functions                                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [m, mi, mz, mzi, dim] = computeGradients(M,z,empty)
%
% This function computes the gradients of a matrix M with respect the the
% variables in z, and then returns both the matrix and its gradient as
% column vectors of their non-zero elements, along with the linear indicies
% to unpack them. It also simplifies m and mz.
%
% INPUTS:
%   M = [na, nb] = symbolic matrix
%   z = [nc, 1] = symbolic vector
%
% OUTPUTS:
%   m = [nd, 1] = symbolic vector of non-zero elements in M
%   mi = [nd, 1] = linear indicies to map m --> [na,nb] matrix
%   mz = [ne, 1] = symbolic vector of non-zero elements in Mz
%   mzi = [ne, 1] = linear indicies to map mz --> [na,nb,nc] array
%   dim = [3,1] = [na,nb,nc] = dimensions of 3d version of mz
%

[na, nb] = size(M);
nc = size(z,1);
M = simplify(M);

mz2 = jacobian(M(:),z);  %Compute jacobian of M, by first reshaping M to be a column vector
mz3 = reshape(mz2,na,nb,nc); %Expand back out to a three-dimensional array
mz3 = simplify(mz3);

% Extract non-zero elements to a column vector:
mi = find(M);
m = M(mi);
mzi = find(mz3);
mz = mz3(mzi); mz = mz(:);  %Collapse to a column vector
dim = [na,nb,nc];

% Pad any constant terms with "empty" to permit vectorization:
m = vectorizeHack(m, z, empty);
mz = vectorizeHack(mz, z, empty);

end

function x = vectorizeHack(x, z, empty)
%
% This function searches for any elements of x that are not dependent on
% any element of z. In this case, the automatically generated code will
% fail to vectorize properly. One solution is to add an array of zeros
% (empty) to the element.
%
% x = column vector of symbolic expressions
% z = column vector of symbolic variables
% z = symbolic variable, which the user will set equal to zero.
%

% Compute dependencies
g = jacobian(x,z);

% Check for rows of x with no dependence on z
[n,m] = size(g);
idxConst = true(n,1);
for i=1:n
    for j=1:m
        if ~isequal(sym(0),g(i,j))
            idxConst(i) = false;
            break;
        end
    end
end

% Add empty to those enteries
x(idxConst) = x(idxConst) + empty;

end

author: MatthewPeterKelly

repo: OptimTraj


text:
function check_cluster_data(cl)
% loads the first 5 images from the first voxel
%
% :Usage:
% ::
%
%     check_cluster_data(cl)
%
% ..
%    tor wager
%    needs Tom's orthviews
% ..

%cl(1).imnames(1:5,:)
cl(1).XYZ(:,1:5), cl(1).XYZmm(:,1:5),
spm_orthviews('Reposition',cl(1).XYZmm(:,1))
%cl(1).all_data(1:5,1)
cl(1).raw_data(1:5,1,2)

return

author: canlab

repo: CanlabCore

save_path: github-repos/MATLAB/canlab-CanlabCore


text:
function eps = leviCivita
% the Levi Civita permutation tensor
%
% Syntax
%   eps = tensor.leviCivita
%

eps = zeros(3,3,3);
eps(1,2,3) = 1;
eps(3,1,2) = 1;
eps(2,3,1) = 1;

eps(1,3,2) = -1;
eps(3,2,1) = -1;
eps(2,1,3) = -1;

eps = tensor(eps,'rank',3,'name','Levi Cevita');

end

author: mtex-toolbox

repo: mtex

save_path: github-repos/MATLAB/mtex-toolbox-mtex


text:
function temp = lpass(E_map)
n = size(E_map,1);
t = E_map;
sn = 33;

vec = linspace(-pi,pi,sn)'*ones(1,sn);
gaus = exp((-vec.^2- (vec').^2)/2)/sqrt(2*pi);

span = [fliplr(fliplr(t).') flipud(t) t.';fliplr(t) t fliplr(t);t.' flipud(t) fliplr(fliplr(t).')];
convdata = conv2(span,gaus);
l = (size(convdata,2)-n)/2;
temp = convdata((l+1):(end-l) , (l+1):(end-l));
temp = temp./sum(sum(temp)).*sum(sum(E_map));

author: JeffFessler

repo: mirt

save_path: github-repos/MATLAB/JeffFessler-mirt


text:
%BRIGHTEDGES  Bright edges detector
%
%     edge = cv.BrightEdges(img)
%     edge = cv.BrightEdges(img, 'OptionName',optionValue, ...)
%
% ## Input
% * __img__ input color image.
%
% ## Output
% * __edge__ output edge image.
%
% ## Options
% * __Contrast__ default 1
% * __ShortRange__ default 3
% * __LongRange__ default 9
%
% The function implements a new way of detecting edges used is low resolution
% image object recognition in real projects (e.g. 50cm per pixel). It corrects
% surfaces for objects partially under a lighting shadow, and reveal low
% visibility edges. The result is used to feed further object detection
% processes particularly in context where small edges or details are important
% for object discrimination.
%
% The function provides an implementation of an equalized absolute difference
% of blurs, and an optional further treatment to contrast edges based on
% finding local minimum along at least two directions. The local minimum
% detection contrast is a parameter, using 10 as default (10 on 255 maximum).
% The kernel size for the gaussian and the average blur can be passed as
% parameters too.
%
% See also: cv.Canny, cv.blur, cv.GaussianBlur, cv.equalizeHist
%

author: kyamagu

repo: mexopencv

save_path: github-repos/MATLAB/kyamagu-mexopencv


text:
%% interp_polar
% Below is a demonstration of the features of the |interp_polar| function

%%
clear; close all; clc;

%% Syntax
% |[Ri]=interp_polar(TZ,radiusData,TZi,interp_method);|

%% Description
% UNDOCUMENTED
%% Examples
%
%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.

author: gibbonCode

repo: GIBBON

save_path: github-repos/MATLAB/gibbonCode-GIBBON


text:
function p = polyfitweighted2(x,y,z,n,w)
% polyfitweighted2.m
% -------------------
%
% Find a least-squares fit of 2D data z(x,y) with an nth order
% polynomial, weighted by w(x,y) .
%
% By S.S. Rogers (2006)
%
% Usage
% ------
%
% P = polyfitweighted2(X,Y,Z,N,W) finds the coefficients of a polynomial
% P(X,Y) of degree N that fits the data Z best in a least-squares
% sense. P is a row vector of length (N+1)*(N+2)/2 containing the
% polynomial coefficients in ascending powers, 0th order first.
%
%   P = [p00 p10 p01 p20 p11 p02 p30 p21 p12 p03...]
%
% e.g. For a 3rd order fit,
% the regression problem is formulated in matrix format as:
%
%   wZ = V*P    or
%
%                      2       2   3   2     2      3
%   wZ = [w  wx  wy  wx  xy  wy  wx  wx y  wx y   wy ]  [p00
%                                                        p10
%                                                        p01
%                                                        p20
%                                                        p11
%                                                        p02
%                                                        p30
%                                                        p21
%                                                        p12
%                                                        p03]
%
% *Note:* P is not in the format of standard Matlab 1D polynomials. Use
% polval2.m to evaluate the polynomial in this format, at given values of
% x,y.
%
% X,Y must be vectors
% Z,W must be 2D arrays of size [length(X) length(Y)]
%
% based on polyfit.m by The Mathworks Inc. - see doc polyfit for more details
%
% Class support for inputs X,Y,Z,W:
%      float: double, single

x = x(:);
y = y(:);

lx=length(x);
ly=length(y);

if ~isequal(size(z),size(w),[ly lx])
    error('polyfitweighted2:XYSizeMismatch',...
         [' X,Y *must* be vectors' ...
          '  Z,W *must* be 2D arrays of size [length(X) length(Y)]'])
end

y=y*ones(1,lx);
x=ones(ly,1)*x';
x = x(:);
y = y(:);
z = z(:);
w = w(:);

pts=length(z);

% Construct weighted Vandermonde matrix.
V=zeros(pts,(n+1)*(n+2)/2);
V(:,1) = w;
%V(:,1) = ones(pts,1);
ordercolumn=1;
for order = 1:n
    for ordercolumn=ordercolumn+(1:order)
        V(:,ordercolumn) = x.*V(:,ordercolumn-order);
    end
    ordercolumn=ordercolumn+1;
    V(:,ordercolumn) = y.*V(:,ordercolumn-order-1);
end

% Solve least squares problem.
[Q,R] = qr(V,0);
ws = warning('off','all');
p = R\(Q'*(w.*z));    % Same as p = V\(w.*z);
warning(ws);
if size(R,2) > size(R,1)
   warning('polyfitweighted2:PolyNotUnique', ...
       'Polynomial is not unique; degree >= number of data points.')
elseif condest(R) > 1.0e10
        warning('polyfitweighted2:RepeatedPointsOrRescale', ...
            ['Polynomial is badly conditioned. Remove repeated data points\n' ...
            '         or try centering and scaling as described in HELP POLYFIT.'])
end
%r = z - (V*p)./w;
p = p.';          % Polynomial coefficients are row vectors by convention.


text:
function [dat] = read_besa_mul(filename)

% READ_BESA_MUL reads data from a BESA multiplexed (*.mul) file
%
% Use as
%   dat = read_besa_mul(filename);

% Copyright (C) 2005, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

dat = [];
fid = fopen(filename, 'rt');

% According to the BESA documentation, the ASCII Multiplexed Format is as
% follows:
%
% The first of two header lines contains similar information to that of the
% BESA ASCII file:
%
% TimePoints= 200 Channels= 27 BeginSweep[ms]= -500.00
% SamplingInterval[ms]= 5.000 Bins/uV= 1.000 SegmentName=Condition1
%
% Note that the item 'SegmentName' is missing if no segment comment is
% specified when writing a segment to file.
%
% If an epoch of a continuous EEG is exported in ASCII multiplexed format,
% the first header line contains the additional item 'Time', which
% indicates the daytime of the first sample in the exported segment:
%
% TimePoints= 200 Channels= 27 BeginSweep[ms]= 0.00 SamplingInterval[ms]=
% 5.000 Bins/uV= 1.000 Time=22:02:53 SegmentName=Segment1

hdr1 = fgetl(fid);
% split the first header line into separate elements
%!!! 2009/09/25
%tmp = tokenize(hdr1, ' ');
tmp = tokenize(hdr1, ' ',1);
for i=1:length(tmp)
  % extract the information from each element
  dum = tokenize(tmp{i}, '=');
  var = dum{1}; % this is the name of the header variable
  val = dum{2}; % this is the value of the header variable
  var(var=='/') = '_';  % replace characters that are not valid in a structure element name
  var(var=='[') = '_';  % replace characters that are not valid in a structure element name
  var(var==']') = '_';  % replace characters that are not valid in a structure element name
  var(var=='.') = '_';  % replace characters that are not valid in a structure element name
  num = str2num(val);
  if ~isempty(num)
    dat = setfield(dat, var, num);
  else
    dat = setfield(dat, var, val);
  end
end

% The second line of the header contains labels for each channel, which may
% be either the original channel names, or the names of the channels of the
% current montage, e.g.
%
% O1 Oz P3 T5 T3 C3 F7 F3 Fp1 Fz Cz Pz Fp2 F4 F8 C4 T4 T6 P4 Fpz O2 M2 M1
% F10 F9 T10 T9
%
% Each subsequent line contains values for all 'Channels' at one time
% point, in floating point or scientific format. Values are given for the
% current or the original montage, selected as described above.
%
% Labels for source montages have the following form: 'TAr-L'.
%
% The first two letters indicate the head region:
%
% The small letter indicates in part the orientation: r=radial,
% t=tangential, and in part the relative location of the basal temporal
% source: l=lateral, m=mesial.
%
% The final letter after the hyphen indicates L=left, M=middle, R=right.

hdr2 = fgetl(fid);
% split the second header line into channel/source labels
%!!! 2009/09/25
%dat.label = tokenize(hdr2, ' ');
dat.label = tokenize(hdr2, ' ',1);

% read the actual data
dat.data = fscanf(fid, '%g');
dat.data = reshape(dat.data, [dat.Channels dat.TimePoints]);

fclose(fid);
return


text:
% JM: 2006/07/19
% viewer for data Tc on domain Omega

function varargout = viewImage(varargin);
% this is just output and hence not commented; fell free to improve (-;
% function varargout = viewImage(Tc,Omega,m,varargin);

persistent PARA
% -----------------------------------------------------------------------------
if nargin == 0,
  varargout{1} = PARA;
  return;
end;
% set persistent parameter
if nargin >= 1 & strcmp(varargin{1},'set'),

  if strcmp(varargin{2},'para'),
    viewPara = varargin{3};
    fn = fieldnames(viewPara);
    for j=1:length(fn),
      para{2*j-1} = fn{j};
      para{2*j}   = getfield(viewPara,fn{j});
    end;
    viewImage('set',para{:})
    return;
  end;

  % default parameter
  fprintf('%s: set persistent parameter\n',mfilename)
  PARA.viewer   = [];
  PARA.figname  = [];
  PARA.fig      = 1;
  PARA.sub      = [1,1,1];
  PARA.name     = '';
  PARA.plots    = 1;
  PARA.scale    = 0;
  PARA.invert   = 0;

  varargin(1) = [];
  % overwrite default parameter
  for j=1:length(varargin)/2,
    field = varargin{2*j-1};
    value = varargin{2*j};
    PARA  = setfield(PARA,field,value);
  end;
  if nargout > 0, varargout{1} = PARA;  end;
  return;
end;
% -----------------------------------------------------------------------------

% -----------------------------------------------------------------------------
% start to work

Tc    = varargin{1};
Omega = varargin{2};
m     = varargin{3};
varargin(1:3) = [];

% start with global parameters and update
para = PARA;
for j=1:length(varargin)/2,
  field = varargin{2*j-1};
  value = varargin{2*j};
  para  = setfield(para,field,value);
end;

if ~para.plots, return;  end;          % do nothing, just return;

% build parameter list
fields = fieldnames(para);
for j=1:length(fields);
  optn{2*j-1} = fields{j};
  optn{2*j}   = getfield(para,fields{j});
end;

if isfield(para,'scale') & para.scale,
  Tc = 255/max(Tc)*Tc;
end;

if isfield(para,'invert') & para.invert,
  Tc = max(Tc)-Tc;
end;

% setup figure

fig = figure(para.fig);
subplot(para.sub(1),para.sub(2),para.sub(3)); cla;

if ~isempty(para.figname),
  set(fig,'numbertitle','off','name',sprintf('[JM-%d]: %s',fig,para.figname));
end;

% call the viewer

fh = feval(para.viewer,Tc,Omega,m,optn{:});

str = sprintf('%s',para.name);
title(str);

if nargout>0,
  varargout{1} = fh;
end;

return;
%
% if nargout>0,  varargout{1} = fh;  end;
% return;
% %==============================================================================
%
%
% function p = volview(I,Omega,m,varargin)
%
% I = reshape(I,m);
% % I = flipdim(I,3);
% isovalue    = 0;
% viewpoint   = [-37.5,30];
% facecolor   = .75*[1,1,1];
% facealpha   = .8;
%
% %% overwrite default parameter
% for k=1:1:length(varargin)/2,
%   %disp([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
%   eval([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
% end;
%
% I(I<isovalue) = 0;
%
% p(1) = patch(isosurface(I,isovalue));
%
% switch computer,
% case 'PCWIN',
%   set(p(1),...
%     'FaceColor',facecolor,...
%     'EdgeColor','none');
% otherwise,
%   set(p(1),...
%     'FaceColor',facecolor,...
%     'EdgeColor','none',...
%     'FaceAlpha',facealpha);
% end;
%
% isonormals(I,p(1))
%
% p(2) = patch(isocaps(I,isovalue));
%
% set(p(2),...
%   'FaceColor','interp',...
%   'EdgeColor','none');
%
% view(viewpoint);
% axis equal
% axis([1,size(I,2),1,size(I,1),1,size(I,3)]);
% camlight; lighting phong
% material dull
% drawnow
% %==============================================================================
%
% %==============================================================================
% function ih = montage(I,Omega,m,varargin)
%
% I = reshape(I,m);
%
% threshold = 0;
% framesx   = [];
% framesy   = [];
% direction = 'x';
% number    = 'off';
% colmap    = [];
% %% overwrite default parameter
% for k=1:1:length(varargin)/2,
%   %disp([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
%   eval([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
% end;
%
% if threshold ~= 0,
%   I(I<threshold) = 0;
% end;
%
% I = squeeze(I);
% l = length(size(I));
%
% fig = gcf;
% switch l,
%   case 1,
%     hh = plot(1:size(I,1),I);
%   case 2,
%     hh = subimage(I,colmap);
%     if ~isempty(colmap), colormap(colmap); end;
%   case 3,
%     [m1,m2,m3] = size(I);
%
%     switch direction,
%       case 'x',
%         aa = m1; b1 = m2; b2 = m3; I1 = 1:m2; I2 = 1:m3;
%       case 'y',
%         aa = m2; b1 = m1; b2 = m3; I1 = 1:m1; I2 = 1:m3;
%       case 'z',
%         aa = m3; b1 = m1; b2 = m2; I1 = 1:m1; I2 = 1:m2;
%
%       otherwise, jmerror(direction);
%     end;
%
%     if isempty(framesx) & isempty(framesy),
%       framesx = ceil(sqrt(aa));
%       framesy = ceil(aa/framesx);
%     elseif isempty(framesx),
%       framesx = ceil(aa/framesy);
%     else
%       framesy = ceil(aa/framesx);
%     end;
%
%     %   fprintf('image is %dx%dx%d, frames [%d,%d]\n',...
%     %     m1,m2,m3,framesx,framesy);
%
%     B = zeros(b1*framesx,b2*framesy);
%
%     a1 = 0; a2 = 0;
%     for j=1:aa,
%       switch direction,
%         case 'x',  P = squeeze(I(j,:,:));
%         case 'y',  P = squeeze(I(:,j,:));
%         case 'z',  P = squeeze(I(:,:,j));
%       end;
%       B(a1+I1,a2+I2) = P;
%       a2 = a2 + b2;
%       if ~rem(j,framesy), a1 = a1 + b1; a2 = 0; end;
%     end;
%
%     ih = image(B);
%     if ~isempty(colmap), colormap(colmap); end;
%     hold on
%
%     if strcmp(number,'on'),
%       a1 = 0; a2 = 0;
%       for j=1:aa,
%         text(a2+10,a1+20,['#',int2str(j)],'fontsize',5,'color','r');
%         a2 = a2 + b2;
%         if ~rem(j,framesy), a1 = a1 + b1; a2 = 0; end;
%       end;
%     end;
%
%     for j=0:framesx,
%       plot(0.5+[0,framesy*b2],0.5+b1*j*[1,1],'b-');
%     end;
%     for j=0:framesy,
%       plot(0.5+b2*j*[1,1],0.5+[0,framesx*b1],'b-');
%     end;
%
%     axis equal
%     axis([0 framesy*b2+1, 0, framesx*b1+1])
%     axis off
%     hold off;
%   otherwise,
%     jmerror('dimension=?');
% end;
% % ==============================================================================
%
% %==============================================================================
function  PARA = setPARA(varargin)
%fprintf('%s\n',mfilename)
PARA = [];

% overwrite default parameter
for k=1:1:length(varargin)/2,
  field = varargin{2*k-1};
  value = varargin{2*k};
  PARA = setfield(PARA,field,value);
end;

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
function x = AngNormalize2(x)
%ANGNORMALIZE2  Reduce any angle to range [-180, 180)
%
%   X = ANGNORMALIZE(X) reduces arbitrary angles to the range [-180, 180).
%   X can be any shape.

  x = AngNormalize(mod(x, 360));
end


text:
function pass = test_times(pref)
% Test chebfun3/times

if ( nargin == 0)
    pref = chebfunpref;
end
tol = 1e4*pref.cheb3Prefs.chebfun3eps;

f = chebfun3(@(x,y,z) cos(x.*y.*z));
h = chebfun3(@(x,y,z) 2*cos(x.*y.*z));
k = chebfun3(@(x,y,z) cos(x.*y.*z).^2);

pass(1) = norm(f.*2 - h) < tol;
pass(2) = norm(f*2 - h) < tol;
pass(3) = norm(2*f - h) < tol;
pass(4) = norm(2.*f - h) < tol;
pass(5) = norm(f.^2 - k) < tol;
pass(6) = norm(f.*f - k) < tol;

ff = @(x,y,z) cos(x.*y.*z);
gg = @(x,y,z) x + y + z + x.*y.*z;
dom = [-1 1 -1 1 -1 1];
f = chebfun3(ff, dom);
g = chebfun3(gg, dom);
FtimesG = chebfun3(@(x,y,z) ff(x,y,z).*gg(x, y, z), dom);
tolj = norm(dom, inf) * tol;
pass(7) = norm(f.*g - FtimesG) < tolj;

dom = [-2 2 -2 2 -2 2];
f = chebfun3(ff, dom);
g = chebfun3(gg, dom);
FtimesG = chebfun3(@(x,y,z) ff(x,y,z).*gg(x, y, z), dom);
tolj = norm(dom, inf) * tol;
pass(8) = norm(f.*g - FtimesG) < tolj;

dom = [0 pi 0 pi -pi/2 pi/2];
f = chebfun3(ff, dom);
g = chebfun3(gg, dom);
FtimesG = chebfun3(@(x,y,z) ff(x,y,z).*gg(x, y, z), dom);
tolj = norm(dom, inf) * tol;
pass(9) = norm(f.*g - FtimesG) < tolj;

end


text:
function [u1, u2, u3, u4] = controller(current_state, desired_state, model_param, K)

J = model_param.I; % Moment of inertia wrt body frame

% position error
ep = current_state(1:3)-desired_state.pos;

% velocity error
ev = current_state(4:6)-desired_state.vel;

% desired force, F_des
Kp = K.Kp;
Kv = K.Kv;
KR = K.KR;
K_omega = K.K_omega;
m = model_param.mass;
g = model_param.grav;
Fd = -Kp.*ep -Kv.*ev + m*g*[0 0 1]' + m*desired_state.acc;

% desired input u1
yaw = current_state(9);
roll = current_state(7);
pitch = current_state(8);
R = ROTZ(yaw)*ROTX(roll)*ROTY(pitch); % Current rotation
% current z-axis in body frame
zb = R(:,3);
u1 = Fd'*zb;

current_acc = -g*[0 0 1]'+u1*zb;

% accel error
ea = current_acc-desired_state.acc;
% Desired force derivative
Fd_dot = -Kp*ev -Kv*ea + m*desired_state.jerk;
% desired rotation, Rd = [xbd ybd zbd]
zbd = Fd/norm(Fd);

% ybd = zbd X xcd/norm(zbd X xcd)
% xcd
yawd = desired_state.yaw;
yawd_dot = desired_state.yawdot;
yawd_2dot = desired_state.yawddot;

xcd = [cos(yawd) sin(yawd) 0]';
ybd = hat_optr(zbd)*xcd/norm(hat_optr(zbd)*xcd);
% xbd = ybd X zbd
xbd = hat_optr(ybd)*zbd;
Rd1 = [xbd ybd zbd];
Rd2 = [-xbd -ybd zbd];

% eR orientation error
R1 = (Rd1'*R - R'*Rd1);
R2 = (Rd2'*R - R'*Rd2);

eR1 = 1/2*vee_optr(R1);
eR2 = 1/2*vee_optr(R2);
% if norm(eR1) >= norm(eR2)
%     Rd = Rd2;
%     eR = eR2;
%     xbd = -xbd;
%     ybd = -ybd;
% else
    Rd = Rd1;
    eR = eR1;
% end

current_omega = current_state(10:12);

% Desired omega^ = Rd'*R
Fd_norm_dot = Fd'*Fd_dot/norm(Fd);
zbd_dot = (Fd_dot*norm(Fd) - Fd*Fd_norm_dot)/norm(Fd)^2;
xcd_dot = [-sin(yawd) cos(yawd) 0]'*desired_state.yawdot;

zbd_x_xcd_dot = hat_optr(zbd_dot)*xcd + hat_optr(zbd)*xcd_dot;
zbd_xcd_norm_dot = (hat_optr(zbd)*xcd)'*(zbd_x_xcd_dot)/norm(hat_optr(zbd)*xcd);

ybd_dot = (zbd_x_xcd_dot*norm(hat_optr(zbd)*xcd) - hat_optr(zbd)*xcd*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;
xbd_dot = hat_optr(ybd_dot)*zbd + hat_optr(ybd)*zbd_dot;

Rd_dot = [xbd_dot ybd_dot zbd_dot];
wd_hat = Rd'*Rd_dot;
wd = vee_optr(wd_hat);
% error
ew = current_omega - R'*Rd*wd;

% Desired angular accerelation^ = Rd_dot'*Rd_dot + Rd'*Rd_2dot;
R_dot = R*hat_optr(current_omega);
zb_dot = R_dot(:,3);
u1_dot = Fd_dot'*zb + Fd'*zb_dot;
current_jerk = (u1_dot*zb + u1*zb_dot)/m;
ej = current_jerk - desired_state.jerk;
Fd_2dot = -Kp*ea -Kv*ej + m*desired_state.snap;
zbd_2dot = (Fd_2dot*norm(Fd)-Fd_dot*Fd_norm_dot)/norm(Fd)^2 -...
           (((Fd_dot'*Fd_dot + Fd'*Fd_2dot)*Fd + (Fd'*Fd_dot)*Fd_dot)*norm(Fd)^3 -...
           (Fd'*Fd_dot)*Fd * 3*norm(Fd)*(Fd'*Fd_dot))/norm(Fd)^4;

% ybd_2dot
xcd_2dot = [-cos(yawd)*yawd_dot^2-sin(yawd)*yawd_2dot, -sin(yawd)*yawd_dot^2+cos(yawd)*yawd_2dot, 0]';

ybd_2dot_1 = ((hat_optr(zbd_2dot)*xcd + hat_optr(zbd)*xcd_2dot)*norm(hat_optr(zbd)*xcd)-...
             zbd_x_xcd_dot*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;

zbd_x_xcd_2dot = hat_optr(zbd_2dot)*xcd + 2*hat_optr(zbd_dot)*xcd_dot + hat_optr(zbd)*xcd_2dot;
zbd_xcd_norm_2dot = ((zbd_x_xcd_dot'*zbd_x_xcd_dot + (hat_optr(zbd)*xcd)'*zbd_x_xcd_2dot)*norm(hat_optr(zbd)*xcd)-...
                    ((hat_optr(zbd)*xcd)'*zbd_x_xcd_dot)*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;

ybd_2dot_2 = ((zbd_x_xcd_dot*zbd_xcd_norm_dot + hat_optr(zbd)*xcd*zbd_xcd_norm_2dot)*norm(hat_optr(zbd)*xcd)^2-...
              2*hat_optr(zbd)*xcd*norm(hat_optr(zbd)*xcd)*zbd_xcd_norm_dot^2)/norm(hat_optr(zbd)*xcd)^4;

ybd_2dot = ybd_2dot_1 - ybd_2dot_2;

xbd_2dot = hat_optr(ybd_2dot)*zbd + 2*hat_optr(ybd_dot)*zbd_dot + hat_optr(ybd)*zbd_2dot;

Rd_2dot = [xbd_2dot ybd_2dot zbd_2dot];
wd_dot_hat = Rd_dot'*Rd_dot + Rd'*Rd_2dot;
wd_dot = vee_optr(wd_dot_hat);

% Moment
M = -KR.*eR -K_omega.*ew + hat_optr(current_omega)*J*current_omega -J*(hat_optr(current_omega)*R'*Rd*wd - R'*Rd*wd_dot);

u2 = M(1);u3 = M(2); u4 = M(3);

author: yorgoon

repo: minimum-snap-geometric-control


text:
function varargout = pexp(varargin)
%PEXP
%
% y = PEXP(x)
%
% Defines perspective exp, x(1)*exp(x(2)/x(1)) on x(1)>0
%
% Alternatively
%
% y = PEXP(x,y) to define x*exp(y/x) on x>0
%
% Implemented as either evalutation based-nonlinear operator, or
% represented using exponential cones depending on solver. Hence, the
% convexity of this function is exploited to perform convexity analysis and
% rigorous modelling.
%
% See also ENTROPY, LOGSUMEXP, CROSSENTROPY, KULLBACKLEIBLER, EXPCONE

switch class(varargin{1})

    case 'double'

        if nargin == 2
            varargin{1} = [varargin{1};varargin{2}];
        end
        if nargin == 1 && ~isequal(prod(size(varargin{1})),2)
            error('PEXP only defined for 2x1 arguments');
        end
        x = varargin{1};

        varargout{1} = x(1)*exp(x(2)/x(1));

    case 'sdpvar'

        if nargin == 2
            varargin{1} = [varargin{1};varargin{2}];
        end
        if ~isequal(prod(size(varargin{1})),2)
            error('PEXP only defined for 2x1 arguments');
        else
            varargout{1} = yalmip('define',mfilename,varargin{1});
        end

    case 'char'

        operator = CreateBasicOperator('convex','positive','callback');
        operator.range = [0 inf];
        operator.derivative = @derivative;
        operator.convexhull = @convexhull;
        operator.bounds = @bounds;

        varargout{1} = [varargin{3}(1) >= 0];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error([upper(mfilename) ' called with weird argument']);
end

function [L,U] = bounds(xL,xU)
x1 = [xL(1);xL(2)];
x2 = [xU(1);xL(2)];
x3 = [xL(1);xU(2)];
x4 = [xU(1);xU(2)];
L = min([pexp(x1) pexp(x2) pexp(x3) pexp(x4)]);
U = max([pexp(x1) pexp(x2) pexp(x3) pexp(x4)]);

function dp = derivative(x)
z = x(2)/x(1);
dp = [exp(z)-z*exp(z);exp(z)];

function [Ax,Ay,b,K] = convexhull(xL,xU)
x1 = [xL(1);xL(2)];
x2 = [xU(1);xL(2)];
x3 = [xL(1);xU(2)];
x4 = [xU(1);xU(2)];
x5 = (xL+xU)/2;
f1 = pexp(x1);
f2 = pexp(x2);
f3 = pexp(x3);
f4 = pexp(x4);
f5 = pexp(x5);
df1 = derivative(x1);
df2 = derivative(x2);
df3 = derivative(x3);
df4 = derivative(x4);
df5 = derivative(x5);
[Ax,Ay,b,K] = convexhullConvex2D(x1,f1,df1,x2,f2,df2,x3,f3,df3,x4,f4,df4,x5,f5,df5);

author: yalmip

repo: YALMIP

save_path: github-repos/MATLAB/yalmip-YALMIP


text:
%% Export calibration data (corners + 3D coordinates) to
%% text files (in Willson-Heikkila's format or Zhang's format)

%% Thanks to Michael Goesele (from the Max-Planck-Institut) for the original suggestion
%% of adding this export function to the toolbox.

if ~exist('n_ima'),
   fprintf(1,'ERROR: No calibration data to export\n');

else

    if n_ima == 0,
        fprintf(1,'ERROR: No calibration data to export\n');
        return;
    end;

	check_active_images;

	check_extracted_images;

	check_active_images;

   fprintf(1,'Tool that exports calibration data to Willson-Heikkila or Zhang formats\n');

   qformat = -1;

   while (qformat ~=0)&(qformat ~=1),

      fprintf(1,'Two possible formats of export: 0=Willson and Heikkila, 1=Zhang\n')
      qformat = input('Format of export (enter 0 or 1): ');

      if isempty(qformat)
         qformat = -1;
      end;

      if (qformat ~=0)&(qformat ~=1),

         fprintf(1,'Invalid entry. Try again.\n')

      end;

   end;

   if qformat == 0,

		fprintf(1,'\nExport of calibration data to text files (Willson and Heikkila''s format)\n');
		outputfile = input('File basename: ','s');

		for kk = ind_active,

   		eval(['X_kk = X_' num2str(kk) ';']);
      	eval(['x_kk = x_' num2str(kk) ';']);

         Xx = [X_kk ; x_kk]';

			file_name = [outputfile num2str(kk)];

			disp(['Exporting calibration data (3D world + 2D image coordinates) of image ' num2str(kk) ' to file ' file_name '...']);

         eval(['save ' file_name ' Xx -ASCII']);

   	end;

   else

      fprintf(1,'\nExport of calibration data to text files (Zhang''s format)\n');
      modelfile = input('File basename for the 3D world coordinates: ','s');
      datafile = input('File basename for the 2D image coordinates: ','s');

      for kk = ind_active,

   		eval(['X_kk = X_' num2str(kk) ';']);
         eval(['x_kk = x_' num2str(kk) ';']);

         if ~exist(['n_sq_x_' num2str(kk)]),
            n_sq_x = 1;
            n_sq_y = size(X_kk,2);
         else
            eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
         	eval(['n_sq_y = n_sq_y_' num2str(kk) ';']);
         end;

 	      X = reshape(X_kk(1,:)',n_sq_x+1,n_sq_y+1)';
 	      Y = reshape(X_kk(2,:)',n_sq_x+1,n_sq_y+1)';
         XY = reshape([X;Y],n_sq_y+1,2*(n_sq_x+1));

         x = reshape(x_kk(1,:)',n_sq_x+1,n_sq_y+1)';
 	      y = reshape(x_kk(2,:)',n_sq_x+1,n_sq_y+1)';
         xy = reshape([x;y],n_sq_y+1,2*(n_sq_x+1));

         disp(['Exporting calibration data of image ' num2str(kk) ' to files ' modelfile num2str(kk) '.txt and ' datafile num2str(kk) '.txt...']);

         eval(['save ' modelfile num2str(kk) '.txt XY -ASCII']);
         eval(['save ' datafile num2str(kk) '.txt xy -ASCII']);

   	end;

end;

fprintf(1,'done\n');

end;

repo: ekfmonoslam


text:
% This .m file "view_maxz.m" plots the maxz LTA values calculated
% with maxzlta.m or other similar values as a color map
% needs re3, gx, gy, stri
%

report_this_filefun(mfilename('fullpath'));

% define size of the plot etc.
%
co = 'w';

% Find out of figure already exists
%
[existFlag,figNumber]=figure_exists('b-value cross-section',1);
newbmapcWindowFlag=~existFlag;

% This is the info window text
%
ttlStr='The Z-Value Map Window                        ';
hlpStr1zmap= ...
    ['                                                '
    ' This window displays seismicity rate changes   '
    ' as z-values using a color code. Negative       '
    ' z-values indicate an increase in the seismicity'
    ' rate, positive values a decrease.              '
    ' Some of the menu-bar options are               '
    ' described below:                               '
    '                                                '
    ' Threshold: You can set the maximum size that   '
    '   a volume is allowed to have in order to be   '
    '   displayed in the map. Therefore, areas with  '
    '   a low seismicity rate are not displayed.     '
    '   edit the size (in km) and click the mouse    '
    '   outside the edit window.                     '
    'FixAx: You can chose the minimum and maximum    '
    '        values of the color-legend used.        '
    'Polygon: You can select earthquakes in a        '
    ' polygon either by entering the coordinates or  '
    ' defining the corners with the mouse            '];
hlpStr2zmap= ...
    ['                                                '
    'Circle: Select earthquakes in a circular volume:'
    '      Ni, the number of selected earthquakes can'
    '      be edited in the upper right corner of the'
    '      window.                                   '
    ' Refresh Window: Redraws the figure, erases     '
    '       selected events.                         '

    ' zoom: Selecting Axis -> zoom on allows you to  '
    '       zoom into a region. Click and drag with  '
    '       the left mouse button. type <help zoom>  '
    '       for details.                             '
    ' Aspect: select one of the aspect ratio options '
    ' Text: You can select text items by clicking.The'
    '       selected text can be rotated, moved, you '
    '       can change the font size etc.            '
    '       Double click on text allows editing it.  '
    '                                                '
    '                                                '];

% Set up the Seismicity Map window Enviroment
%
if newbmapcWindowFlag
    bmapc = figure_w_normalized_uicontrolunits( ...
        'Name','b-value cross-section',...
        'NumberTitle','off', ...
        'MenuBar','none', ...
        'backingstore','on',...
        'Visible','off', ...
        'Position',[ (fipo(3:4) - [600 400]) ZmapGlobal.Data.map_len]);
    % make menu bar
    matdraw

    add_symbol_menu('eqc_plot');

    uicontrol('Units','normal',...
        'Position',[.0 .93 .08 .06],'String','Print ',...
         'Callback','myprint')

    callbackStr= ...
        ['f1=gcf; f2=gpf; set(f1,''Visible'',''off'');close(bmapc);', ...
        'if f1~=f2, figure_w_normalized_uicontrolunits(map);done; end'];

    uicontrol('Units','normal',...
        'Position',[.0 .75 .08 .06],'String','Close ',...
         'Callback','eval(callbackStr)')

    uicontrol('Units','normal',...
        'Position',[.0 .85 .08 .06],'String','Info ',...
         'Callback','zmaphelp(ttlStr,hlpStr1zmap,hlpStr2zmap)')

    options = uimenu('Label',' Select ');
    uimenu(options,'Label','Refresh ', 'Callback','view_bv2')
    uimenu(options,'Label','Select EQ in Circle',...
         'Callback',' h1 = gca;ho=false;cicros;watchoff(bmapc)')
    uimenu(options,'Label','Select EQ in Circle - Overlay existing plot', 'Callback','h1 = gca;ho=true;cicros;watchoff(bmapc)')

    op1 = uimenu('Label',' Tools ');
    uimenu(op1,'Label','Fix color (z) scale', 'Callback','fixax2 ')
    uimenu(op1,'Label','Histogram of b-values', 'Callback','zhist')
    uimenu(op1,'Label','Show  b-value Map', 'Callback',' re3 = old; view_bv2')
    uimenu(op1,'Label','Show  b(mean) map', 'Callback',' re3 = 0.4343./(meg-min(newa(:,6))); view_bv2')
    uimenu(op1,'Label','Show  mean magnitude map', 'Callback',' re3 = meg; view_bv2')
    uimenu(op1,'Label','Show  mag of completness map', 'Callback',' re3 = old1; view_bv2')
    uimenu(op1,'Label','Show Resolution Map', 'Callback','re3 = r; view_bv2')
    uimenu(op1,'Label','Show Grid ',...
         'Callback',' [X,Y] = meshgrid(gx,gy);hold on;plot(X,Y,''+k'')')
    uimenu(op1,'Label','Show Circles ', 'Callback','plotcirc')
    uimenu(op1,'Label','Colormap InvertGray', 'Callback','g=gray; g = g(64:-1:1,:);colormap(g);brighten(.4)')
    uimenu(op1,'Label','shading flat', 'Callback','axes(hzma); shading flat')
    uimenu(op1,'Label','shading interpolated',...
         'Callback','axes(hzma); shading interp')

    uicontrol('Units','normal',...
        'Position',[.92 .80 .08 .05],'String','set ni',...
         'Callback','ni=str2num(get(set_nia,''String''));''String'',num2str(ni);')

    set_nia = uicontrol('style','edit','value',ni,'string',num2str(ni));
    set(set_nia,'Callback',' ');
    set(set_nia,'units','norm','pos',[.94 .85 .06 .05],'min',10,'max',10000);
    nilabel = uicontrol('style','text','units','norm','pos',[.90 .85 .04 .05]);
    set(nilabel,'string','ni:','background',[.7 .7 .7]);

    % tx = text(0.07,0.95,[name],'Units','Norm','FontSize',18,'Color','k','FontWeight','bold');

    tresh = max(max(r)); re4 = re3;
    nilabel2 = uicontrol('style','text','units','norm','pos',[.60 .92 .25 .06]);
    set(nilabel2,'string','MinRad (in km):','background',color_fbg);
    set_ni2 = uicontrol('style','edit','value',tresh,'string',num2str(tresh),...
        'background','y');
    set(set_ni2,'Callback','tresh=str2double(get(set_ni2,''String'')); set(set_ni2,''String'',num2str(tresh))');
    set(set_ni2,'units','norm','pos',[.85 .92 .08 .06],'min',0.01,'max',10000);

    uicontrol('Units','normal',...
        'Position',[.95 .93 .05 .05],'String','Go ',...
         'Callback','think;pause(1);re4 =re3; view_bv2')

end   % This is the end of the figure setup

% Now lets plot the color-map of the z-value
%
figure_w_normalized_uicontrolunits(bmapc)
delete(gca)
delete(gca)
delete(gca)
try
    delete(sizmap)
catch ME
    error_handler(ME, ' ');
end
reset(gca)
cla
hold off
watchon;
set(gca,'visible','off','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on','SortMethod','childorder')

rect = [0.18,  0.00, 0.7, 0.75];
rect1 = rect;

% set values gretaer tresh = nan
%
re4 = re3;
l = r > tresh;
re4(l) = zeros(1,length(find(l)))*nan;

% plot image
%
orient portrait
%set(gcf,'PaperPosition', [2. 1 7.0 5.0])

axes('position',rect)
hold on
pco1 = pcolor(gx,gy,re4);

axis([ min(gx) max(gx) min(gy) max(gy)])
axis image
hold on
shading interp
h = hsv(64);
    h = h(57:-1:1,:);
    colormap(h)

if fre == 1
    caxis([fix1 fix2])
end

ylabel('depth in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)

% ploeqc = plot(newa(:,length(newa(1,:))),-newa(:,7),'.k')

if ~isempty(maix)
    pl = plot(maix,maiy,'*k')
    set(pl,'MarkerSize',12,'LineWidth',2)
end

%if ~isempty(maex)
pl = plot(maex,-maey,'xw')
set(pl,'MarkerSize',10,'LineWidth',2)
%end

set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on','TickDir','out')
h1 = gca;
hzma = gca;

% Create a colobar
%
h5 = colorbar('vert');
set(h5,'Pos',[0.80 0.40 0.02 0.15],...
    'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
%h5 = colorbar('horiz');
%set(h5,'Pos',[0.25 0.05 0.5 0.05],...
%'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)

% Make the figure visible
%
set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on','TickDir','out')

rect = [0.18,  -0.20, 0.7, 0.75];
rect1 = rect;

% set values gretaer tresh = nan
%
re4 = ret;
l = r > tresh;
re4(l) = zeros(1,length(find(l)))*nan;
axes('position',rect)
hold on
pco1 = pcolor(gx,gy,re4);

axis([ min(gx) max(gx) min(gy) max(gy)])
axis image
hold on
shading interp
h = hsv(64);
h = h(57:-1:1,:);
colormap(h)
%end
caxis([20 200])

xlabel('Distance in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
ylabel('depth in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)

% plot overlay
%
% ploeqc = plot(newa(:,length(newa(1,:))),-newa(:,7),'.k')

pl = plot(maex,-maey,'xw')
set(pl,'MarkerSize',10,'LineWidth',2)

set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on','TickDir','out')
h1 = gca;
hzma = gca;

% Create a colobar
%
h5 = colorbar('vert');
set(h5,'Pos',[0.80 0.10 0.02 0.15],...
    'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)

% Make the figure visible
%
set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on','TickDir','out')

rect = [0.18,  0.40, 0.7, 0.75];
axes('position',rect)
hold on
ploeqc = plot(newa(:,length(newa(1,:))),-newa(:,7),'+r');
hold on
set(ploeqc,'Tag','eqc_plot','MarkerSize',8,'LineWidth',1)
hold on

axis([ min(gx) max(gx) min(gy) max(gy)])
axis image
hold on

ylabel('depth in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)

% plot overlay
%
if ~isempty(maix)
    pl = plot(maix,maiy,'*k')
    set(pl,'MarkerSize',12,'LineWidth',2)
end

%if ~isempty(maex)
pl = plot(maex,-maey,'kx')
set(pl,'MarkerSize',10,'LineWidth',2)
%end

set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on','TickDir','out','Color',[1 1 0.6])

figure_w_normalized_uicontrolunits(bmapc);
watchoff(bmapc)
done

author: CelsoReyes


text:
function [ y, symm ] = cvx_s_upper_hankel( m, n, symm )

% CVX_S_UPPER_HANKEL Upper Hankel matrices.

c  = 0 : n - 1;
c  = c( ones( 1, m ), : );
r  = ( 0 : m - 1 )';
r  = r( :, ones( 1, n ) );
v  = abs( r + c ) + 1;
temp = v <= min( m, n );
y = sparse( v( temp ), r( temp ) + m * c( temp ) + 1, 1, min( m, n ), m * n );
symm = false;

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.

author: yu-jiang

repo: radpbook

save_path: github-repos/MATLAB/yu-jiang-radpbook


text:
function [X] = spm_zeros(X)
% fills a cell or structure array with zeros
% FORMAT [X] = spm_zeros(X)
% X  - numeric, cell or stucture array[s]
%__________________________________________________________________________
% Copyright (C) 2005-2013 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_zeros.m 6233 2014-10-12 09:43:50Z karl $

% create zeros structure
%--------------------------------------------------------------------------
X = spm_unvec(zeros(spm_length(X),1),X);

author: spm


text:
function []=irfdisp(n,endo,IRFperiods,IRFt,irf_estimates,D_estimates,gamma_estimates,pref,strctident)

% function []=irfdisp(n,endo,IRFperiods,IRFt,irf_estimates,D_estimates,gamma_estimates,datapath)
% plots the results for the impulse response functions
% inputs:  - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - cell 'endo': list of endogenous variables of the model
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'IRFt': determines which type of structural decomposition to apply (none, Choleski, triangular factorization)
%          - cell 'irf_estimates': lower bound, point estimates, and upper bound for the IRFs
%          - vector 'D_estimates': point estimate (median) of the structural matrix D, in vectorised form
%          - vector 'gamma_estimates': point estimate (median) of the structural disturbance variance-covariance matrix gamma, in vectorised form
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: none

% transpose the cell of records (required for the plot function in order to obtain correctly ordered plots)
irf_estimates=irf_estimates';

% number of identified shocks
if IRFt==1||IRFt==2||IRFt==3
    identified=n; % fully identified
elseif IRFt==4 || IRFt==6 %if the model is identified by sign restrictions or sign restrictions (+ IV)
    identified=size(strctident.signreslabels_shocks,1); % count the labels provided in the sign res sheet (+ IV)
elseif IRFt==5
    identified=1; % one IV shock
    strctident.signreslabels_shocks{1,1}=strcat('IV Shock (',strctident.Instrument,')'); % and generate the sign res label here
    strctident.signreslabels_shocksindex=1; % first shock
end

count=1;
namecount=1;

if pref.plot==1
% create figure for IRFs
irf=figure('Tag','BEARresults');
set(irf,'Color',[0.9 0.9 0.9]);
%set(irf,'position',[0,0,1920,1080])
   if IRFt==1
   irfname='impulse response functions (no structural identifcation)';
   elseif IRFt==2
   irfname='impulse response functions (structural identification by Cholesky ordering)';
   elseif IRFt==3
   irfname='impulse response functions (structural identification by triangular factorisation)';
   elseif IRFt==4
   irfname=['impulse response functions (structural identification by ',strctident.hbartext_signres,strctident.hbartext_favar_signres,strctident.hbartext_zerores,strctident.hbartext_favar_zerores,strctident.hbartext_magnres,strctident.hbartext_favar_magnres,strctident.hbartext_relmagnres,strctident.hbartext_favar_relmagnres,strctident.hbartext_FEVDres,strctident.hbartext_favar_FEVDres,strctident.hbartext_CorrelInstrumentShock,':::',' restrictions)'];
   irfname=erase(irfname,', :::'); % delete the last ,
   elseif IRFt==5
   irfname=['impulse response functions (structural identification by IV (',strctident.Instrument,'))'];
   elseif IRFt==6
   irfname_temp=[strctident.hbartext_signres,strctident.hbartext_favar_signres,strctident.hbartext_zerores,strctident.hbartext_favar_zerores,strctident.hbartext_magnres,strctident.hbartext_favar_magnres,strctident.hbartext_relmagnres,strctident.hbartext_favar_relmagnres,strctident.hbartext_FEVDres,strctident.hbartext_favar_FEVDres,strctident.hbartext_CorrelInstrumentShock,':::',' restrictions)'];
   irfname_temp=erase(irfname_temp,', :::'); % delete the last ,
   irfname=['impulse response functions (structural identification by IV (',strctident.Instrument,') & ',irfname_temp];
   end
   set(irf,'name',irfname);

if IRFt==1||IRFt==2||IRFt==3
for ii=1:n^2
subplot(n,n,ii);
hold on
Xpatch=[(1:IRFperiods) (IRFperiods:-1:1)];
Ypatch=[irf_estimates{ii}(1,:) fliplr(irf_estimates{ii}(3,:))];
IRFpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
set(IRFpatch,'facealpha',0.5);
set(IRFpatch,'edgecolor','none');
plot(irf_estimates{ii}(2,:),'Color',[0.4 0.4 1],'LineWidth',2);
plot([1,IRFperiods],[0 0],'k--');
hold off
minband=min(irf_estimates{ii}(1,:));
maxband=max(irf_estimates{ii}(3,:));
space=maxband-minband;
Ymin=minband-0.2*space;
Ymax=maxband+0.2*space;
set(gca,'XLim',[1 IRFperiods],'YLim',[Ymin Ymax],'FontName','Times New Roman');
% top labels
   if ii<=n
   title(endo{ii,1},'FontWeight','normal','interpreter','none');
   end
% side labels
   if rem((ii-1)/n,1)==0
   ylabel(endo{(ii-1)/n+1,1},'FontWeight','normal','interpreter','none');
   end
end
%% IRFt==4||IRFt==5||IRFt==6
elseif IRFt==4||IRFt==5||IRFt==6
    % subset of irf_estimates and signreslabels that we want to plot
    irf_estimates_shocks=irf_estimates(strctident.signreslabels_shocksindex,:);
    signreslabels_shocks=strctident.signreslabels_shocks;
for jj=1:identified
    for ii=1:n
        count;
        subplot(n,identified,jj+identified*(ii-1));
hold on
Xpatch=[(1:IRFperiods) (IRFperiods:-1:1)];
Ypatch=[irf_estimates_shocks{jj,ii}(1,:) fliplr(irf_estimates_shocks{jj,ii}(3,:))];
IRFpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
set(IRFpatch,'facealpha',0.5);
set(IRFpatch,'edgecolor','none');
plot(irf_estimates_shocks{jj,ii}(2,:),'Color',[0.4 0.4 1],'LineWidth',2);
plot([1,IRFperiods],[0 0],'k--');
hold off
minband=min(irf_estimates_shocks{jj,ii}(1,:));
maxband=max(irf_estimates_shocks{jj,ii}(3,:));
space=maxband-minband;
Ymin=minband-0.2*space;
Ymax=maxband+0.2*space;
set(gca,'XLim',[1 IRFperiods],'YLim',[Ymin Ymax],'FontName','Times New Roman');
% top labels
subplotcolumn=jj+identified*(ii-1);
if subplotcolumn <= identified
title(signreslabels_shocks{subplotcolumn,1},'FontWeight','normal','interpreter','none');
end
if count <= n
   ylabel(endo{namecount,1},'FontWeight','normal','interpreter','none');
   namecount=namecount+1;
end
count = count+1;
end
end
end
% top supertitle
ax=axes('Units','Normal','Position',[.11 .075 .85 .88],'Visible','off');
set(get(ax,'Title'),'Visible','on')
title('Shock:','FontSize',11,'FontName','Times New Roman','FontWeight','normal','interpreter','none');
% side supertitle
ylabel('Response of:','FontSize',12,'FontName','Times New Roman','FontWeight','normal','interpreter','none');
set(get(ax,'Ylabel'),'Visible','on')
    set(irf,'PaperPositionMode','Auto')
% % %      PrintName = strcat('IRFs');
% % %      fname = strcat(pref.datapath, '\results\');
% % %      saveas(irf,[fname,PrintName],'epsc')
% % %      saveas(irf,[fname,PrintName],'png')
end

if IRFt==2 || IRFt==3 || IRFt==4 || IRFt==5 || IRFt==6
% then display the results for D and gamma, if a structural decomposition was selected

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'at');

%print three empty lines
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

%print structural decomposition matrix D
svarinfo1=['D (structural decomposition matrix): posterior estimates'];
fprintf('%s\n',svarinfo1);
fprintf(fid,'%s\n',svarinfo1);

% recover D
D=reshape(D_estimates,n,n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(D))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(D(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

%print two empty lines
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

%print structural disturbances covariance matrix gamma
svarinfo2=['gamma (structural disturbances covariance matrix): posterior estimates'];
fprintf('%s\n',svarinfo2);
fprintf(fid,'%s\n',svarinfo2);

% recover gamma
gamma=reshape(gamma_estimates,n,n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(gamma))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(gamma(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end % pref.plot
fclose(fid);
end

% finally, record results in excel
if pref.results==1
% retranspose the cell of records
irf_estimates=irf_estimates';
bear.data.excelrecord4fcn(identified, IRFperiods, IRFt, endo, strctident, irf_estimates, n, pref)
end

author: european-central-bank

repo: BEAR-toolbox


text:
function [returnVar,msg] = RemoveDCfromDat(fname,nbChan)

% USAGE:
%     RemoveDCfromDat(fbasename,nbChan)
%     This function removes DC from dat files by computing the average of
%     the first 1e6 samples (or less if file is smaller)
% INPUTS:
%     fname: dat file name
%     nbChan: total number of channels in dat file
%
% Adrien Peyrache 2011

fprintf('Removing baseline from %s\n',fname)
try
    infoFile = dir(fname);

    chunk = 1e6;
    nbChunks = floor(infoFile.bytes/(nbChan*chunk*2));
    warning off
    if nbChunks==0
        chunk = infoFile.bytes/(nbChan*2);
    end
    m = memmapfile(fname,'Format','int16','Repeat',chunk*nbChan,'writable',true);
    d = m.Data;
    d = reshape(d,[nbChan chunk]);
    meanD = mean(d,2);
    d = d-int16(meanD*ones(1,chunk));
    m.Data = d(:);
    clear d m

    for ix=1:nbChunks-1
    %    h=waitbar(ix/(nbChunks-1));
        m = memmapfile(fname,'Format','int16','Offset',ix*chunk*nbChan*2,'Repeat',chunk*nbChan,'writable',true);
        d = m.Data;
        d = reshape(d,[nbChan chunk]);
        d = d-int16(meanD*ones(1,chunk));
        m.Data = d(:);
        clear d m
    end
    %close(h)

    newchunk = infoFile.bytes/(2*nbChan)-nbChunks*chunk;

    if newchunk
        m = memmapfile(fname,'Format','int16','Offset',nbChunks*chunk*nbChan*2,'Repeat',newchunk*nbChan,'writable',true);
        d = m.Data;
        d = reshape(d,[nbChan newchunk]);
        d = d-int16(meanD*ones(1,newchunk));
        m.Data = d(:);
     clear d m
    end
    warning on
    returnVar = 1;
    msg = '';

catch
%     keyboard
    returnVar = 0;
    msg = lasterr;
end
clear m

author: buzsakilab

repo: buzcode

save_path: github-repos/MATLAB/buzsakilab-buzcode


text:
function simp_plot_3d ( p, t, expr, bcolor, icolor )

%*****************************************************************************80
%
%% SIMP_PLOT_3D displays a plot of the tetrahedrons that form a mesh in 3D.
%
%  Copyright:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    23 September 2005
%
%  Reference:
%
%    Per-Olof Persson, Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,3), the coordinates of a set of nodes in 3D.
%
%    Input, integer T(NT,1:4), a list of the nodes which make up each
%    tetrahedron in the mesh.
%
%    Input, logical EXPR, an expression which, if supplied, determines which
%    tetrahedrons are to be highlighted.
%
%    Input, real BCOLOR(3), the RGB color to use for faces.
%
%    Input, real ICOLOR(3), the RGB color to use for highlighted faces.
%
  if ( nargin < 4 )
    bcolor = 0.9 * ones ( 1, 3 );
  end

  if ( nargin < 5 )
    icolor = [ 1.0, 0.0, 0.0 ];
  end

  tri1 = surftri ( p, t );

  if ( 2 < nargin & ~isempty ( expr ) )
    incl = find ( eval(expr) );
    t = t(any(ismember(t,incl),2),:);
    tri1 = tri1(any(ismember(tri1,incl),2),:);
    tri2 = surftri ( p, t );
    tri2 = setdiff ( tri2, tri1, 'rows' );
    h = trimesh ( tri2, p(:,1), p(:,2), p(:,3) );
    set ( h, 'FaceColor', icolor, 'EdgeColor', 'k' );
    hold on
  end

  h = trimesh ( tri1, p(:,1), p(:,2), p(:,3) );

  xlabel ( '- X axis -' )
  ylabel ( '- Y axis -' )
  zlabel ( '- Z axis -' )

  set ( h, 'facecolor', bcolor, 'edgecolor', 'k' );
  axis equal

  hold off

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function RtOut = inverseRt(RtIn)

RtOut = [RtIn(1:3,1:3)', - RtIn(1:3,1:3)'* RtIn(1:3,4)];

author: jianxiongxiao

repo: ProfXkit


text:
function [err_res,elerr_res] = diffpost_res(jmp,els,rhsq,hlsq)
%diffpost_res  computes Q1 element residual error estimator
%   [err_res,elerr_res] = diffpost_res(jmp,els,rhsq,hlsq);
%   input
%          jmp          elementwise edge flux jumps
%          els          elementwise edge lengths
%          rhsq         elementwise L2 residual norms
%          hlsq         elementwise areas
%   output
%          err_res      global residual error
%          elerr_res    elementwise residual errors
%
%   IFISS function: DJS; 1 April 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
elerr_res=0.5*sum((els.*els.*jmp.*jmp)')' + hlsq.*rhsq;
err_res=sqrt(sum(elerr_res));
elerr_res=sqrt(elerr_res);
fprintf('computing residual error estimator... ')
fprintf('\nestimated global error (in energy):  %10.6e\n',err_res)
return