text:
function soln = rungeKutta(problem)
% soln = rungeKutta(problem)
%
% This function transcribes a trajectory optimization problem using the
% multiple shooting, with 4th-order Runge Kutta integration
%
% See Bett's book for details on the method
%
% For details on the input and output, see the help file for optimTraj.m
%
% Method specific parameters:
%
%   problem.options.method = 'rungeKutta'
%   problem.options.rungeKutta = struct with method parameters:
%       .nSegment = number of trajectory segments
%       .nSubStep = number of sub-steps to use in each segment
%       .adaptiveDerivativeCheck = 'off' by default. Set to 'on' to enable
%           numerical checks on the analytic gradients, computed using the
%           derivest package, rather than fmincon's internal checks.
%           Derivest is slower, but more accurate than fmincon. Derivest
%           can be downloaded from the Mathworks File Exchange, file id of
%           13490 - Adaptive Robust Numerical Differentation, John D-Errico
%
%
% NOTES:
%
%   Code for computing analyic gradients of the Runge Kutta method was
%   contributed by Will Wehner.
%
%   If analytic gradients are used, then the sparsity pattern is returned
%   in the struct: soln.info.sparsityPattern. View it using spy().
%
%

%To make code more readable
G = problem.guess;
B = problem.bounds;
F = problem.func;
Opt = problem.options;

% Figure out grid size:
nSegment = Opt.rungeKutta.nSegment;
nSubStep = Opt.rungeKutta.nSubStep;
nGridControl = 2*nSegment*nSubStep + 1;
nGridState = nSegment + 1;

% Print out some solver info if desired:
if Opt.verbose > 0
    fprintf('  -> Transcription via 4th-order Runge-Kutta method \n');
    fprintf('        nSegments = %d \n', nSegment);
    fprintf('        nSubSteps = %d \n', nSubStep);
end

% Interpolate the guess at the transcription grid points for initial guess:
guess.tSpan = G.time([1,end]);
guess.tState = linspace(guess.tSpan(1), guess.tSpan(2), nGridState);
guess.tControl = linspace(guess.tSpan(1), guess.tSpan(2), nGridControl);
guess.state = interp1(G.time', G.state', guess.tState')';
guess.control = interp1(G.time', G.control', guess.tControl')';
[zGuess, pack] = packDecVar(guess.tSpan, guess.state, guess.control);

% Unpack all bounds:
tLow = [B.initialTime.low, B.finalTime.low];
xLow = [B.initialState.low, B.state.low*ones(1,nGridState-2), B.finalState.low];
uLow = B.control.low*ones(1,nGridControl);
zLow = packDecVar(tLow,xLow,uLow);

tUpp = [B.initialTime.upp, B.finalTime.upp];
xUpp = [B.initialState.upp, B.state.upp*ones(1,nGridState-2), B.finalState.upp];
uUpp = B.control.upp*ones(1,nGridControl);
zUpp = packDecVar(tUpp,xUpp,uUpp);

%%%% Set up problem for fmincon:
flagGradObj = strcmp(Opt.nlpOpt.GradObj,'on');
flagGradCst = strcmp(Opt.nlpOpt.GradConstr,'on');
if flagGradObj || flagGradCst
    gradInfo = grad_computeInfo(pack);
end
if flagGradObj
    P.objective = @(z)( ...
        myObjGrad(z, pack, F.dynamics, F.pathObj, F.bndObj, gradInfo) );   %Analytic gradients
    [~, objGradInit] = P.objective(zGuess);
    sparsityPattern.objective = (objGradInit~=0)';
else
    P.objective = @(z)( ...
        myObjective(z, pack, F.dynamics, F.pathObj, F.bndObj) );   %Numerical gradients
end

if flagGradCst
    P.nonlcon = @(z)( ...
        myCstGrad(z, pack, F.dynamics, F.pathObj, F.pathCst, F.bndCst, gradInfo) ); %Analytic gradients
    [~,~,cstIneqInit,cstEqInit] = P.nonlcon(zGuess);
    sparsityPattern.equalityConstraint = (cstEqInit~=0)';
    sparsityPattern.inequalityConstraint = (cstIneqInit~=0)';
else
    P.nonlcon = @(z)( ...
        myConstraint(z, pack, F.dynamics, F.pathObj, F.pathCst, F.bndCst) ); %Numerical gradients
end

% Check analytic gradients with DERIVEST package
if strcmp(Opt.rungeKutta.adaptiveDerivativeCheck,'on')
    if exist('jacobianest','file')
        runGradientCheck(zGuess, pack,F.dynamics, F.pathObj, F.bndObj, F.pathCst, F.bndCst, gradInfo);
        Opt.nlpOpt.DerivativeCheck = [];  %Disable built-in derivative check
    else
        Opt.rungeKutta.adaptiveDerivativeCheck = 'cannot find jacobianest.m';
        disp('Warning: the derivest package is not on search path.');
        disp(' --> Using fmincon''s built-in derivative checks.');
    end
end

% Build the standard fmincon problem struct
P.x0 = zGuess;
P.lb = zLow;
P.ub = zUpp;
P.Aineq = []; P.bineq = [];
P.Aeq = []; P.beq = [];
P.solver = 'fmincon';
P.options = Opt.nlpOpt;

%%%% Call fmincon to solve the non-linear program (NLP)
tic;
[zSoln, objVal,exitFlag,output] = fmincon(P);
[tSpan,~,uSoln] = unPackDecVar(zSoln,pack);
nlpTime = toc;

%%%% Store the results:
[tGrid,xGrid,uGrid] = simulateSystem(zSoln, pack, F.dynamics, F.pathObj);
soln.grid.time = tGrid;
soln.grid.state = xGrid;
soln.grid.control = uGrid;

% Quadratic interpolation over each sub-step for the control:
tSoln = linspace(tSpan(1),tSpan(2),nGridControl);
soln.interp.control = @(t)( interp1(tSoln', uSoln', t','pchip')' );

% Cubic spline representation of the state over each substep:
dxGrid = F.dynamics(tGrid,xGrid,uGrid);
xSpline = pwch(tGrid, xGrid, dxGrid);
soln.interp.state = @(t)( ppval(xSpline,t) );

% General information about the optimization run
soln.info = output;
soln.info.nlpTime = nlpTime;
soln.info.exitFlag = exitFlag;
soln.info.objVal = objVal;
if flagGradCst || flagGradObj
    soln.info.sparsityPattern = sparsityPattern;
end

soln.problem = problem;  % Return the fully detailed problem struct

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%                   SUB FUNCTIONS                                   %%%%
%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [decVars,pack] = packDecVar(tSpan,state,control)
%
% This function collapses the time (t), state (x)
% and control (u) matricies into a single vector
%
% INPUTS:
%   tSpan = [1, 2] = time bounds
%   state = [nState, nGridState] = state vector at each grid point
%   control = [nControl, nGridControl] = control vector at each grid point
%
% OUTPUTS:
%   decVars = column vector of 2 + nState*nGridState + nControl*nGridControl) decision variables
%   pack = details about how to convert z back into t,x, and u
%       .nState
%       .nGridState
%       .nControl
%       .nGridControl
%
% NOTES:
% nGridControl = 2*nSegment*nSubStep + 1;
% nGridState = nSegment + 1;
%

[nState, nGridState] = size(state);
[nControl, nGridControl] = size(control);

nSegment = nGridState - 1;
nSubStep = (nGridControl - 1)/(2*nSegment);

xCol = reshape(state, nState*nGridState, 1);
uCol = reshape(control, nControl*nGridControl, 1);

indz = 1:numel(control)+numel(state)+numel(tSpan);

% index of time in decVar
indt = 1:2;

% the z index of the first element of each state over time
indtemp = 2 + (1 : (nState + (2*nSubStep)*nControl ) : numel(control)+numel(state));

% remaining state elements at each time
indx = repmat(indtemp,nState,1) + cumsum(ones(nState,nGridState),1) - 1;

% index of control in decVar
indu = indz;
indu([indt(:);indx(:)])=[];
indu = reshape(indu,nControl,nGridControl);

% pack up decVars
decVars = zeros(numel(indz),1);
decVars(indt(:),1) = tSpan;
decVars(indx(:),1) = xCol;
decVars(indu(:),1) = uCol;

% pack structure
pack.nState = nState;
pack.nGridState = nGridState;
pack.nControl = nControl;
pack.nGridControl = nGridControl;
pack.nSegment = nGridState - 1;
pack.nSubStep = (nGridControl-1)/(2*pack.nSegment);
pack.indt = indt;
pack.indx = indx;
pack.indu = indu;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [tSpan, state, control] = unPackDecVar(decVars,pack)
%
% This function unpacks the decision variables for
% trajectory optimization into the time (t),
% state (x), and control (u) matricies
%
% INPUTS:
%   decVars = column vector of 2 + nState*nGridState + nControl*nGridControl) decision variables
%   pack = details about how to convert z back into t,x, and u
%       .nState
%       .nGridState
%       .nControl
%       .nGridControl
%
% OUTPUTS:
%   tSpan = [1, 2] = time bounds
%   state = [nState, nGridState] = state vector at each grid point
%   control = [nControl, nGridControl] = control vector at each grid point
%

tSpan = [decVars(1),decVars(2)];

% state = reshape(decVars((2+1):(2+nx)), pack.nState, pack.nGridState);
% control = reshape(decVars((2+nx+1):(2+nx+nu)), pack.nControl, pack.nGridControl);

state = decVars(pack.indx);
control = decVars(pack.indu);

% make sure x and u are returned as vectors, [nState,nTime] and
% [nControl,nTime]
state = reshape(state,pack.nState,pack.nGridState);
control = reshape(control,pack.nControl,pack.nGridControl);
end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function cost = myObjective(decVars, pack,dynamics, pathObj, bndObj)
%
% This function unpacks the decision variables, sends them to the
% user-defined objective functions, and then returns the final cost
%
% INPUTS:
%   decVars = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynamics = user-defined dynamics function handle
%   pathObj = user-defined path-objective function
%   bndObj = user-defined boundary objective function
%
% OUTPUTS:
%   cost = scalar cost for this set of decision variables
%
%

% All of the real work happens inside this function:
[t,x,~,~,pathCost] = simulateSystem(decVars, pack, dynamics, pathObj);

% Compute the cost at the boundaries of the trajectory
if isempty(bndObj)
    bndCost = 0;
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    bndCost = bndObj(t0,x0,tF,xF);
end

cost = bndCost + pathCost;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [c, ceq] = myConstraint(decVars, pack, dynamics, pathObj, pathCst, bndCst)
%
% This function unpacks the decision variables, computes the defects along
% the trajectory, and then evaluates the user-defined constraint functions.
%
% INPUTS:
%   decVars = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynamics = user-defined dynamics function handle
%   pathObj = user-defined path-objective function
%   pathCst = user-defined path-constraint function
%   bndCst = user-defined boundary constraint function
%
% OUTPUTS:
%   c = non-linear inequality constraint
%   ceq = non-linear equatlity cosntraint
%
% NOTE:
%   - path constraints are  satisfied at the start and end of each sub-step
%

[t,x,u,defects] = simulateSystem(decVars, pack, dynamics, pathObj);

%%%% Call user-defined constraints and pack up:
[c, ceq] = collectConstraints(t,x,u,...
    defects,...
    pathCst, bndCst);

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [t,x,u,defects,pathCost] = simulateSystem(decVars, pack, dynFun, pathObj)
%
% This function does the real work of the transcription method. It
% simulates the system forward in time across each segment of the
% trajectory, computes the integral of the cost function, and then matches
% up the defects between the end of each segment and the start of the next.
%
% INPUTS:
%   decVars = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynamics = user-defined dynamics function handle
%   pathObj = user-defined path-objective function
%
% OUTPUTS:
%   t = [1 x nGrid] = time vector for the edges of the sub-step grid
%   x = [nState x nGrid] = state vector
%   u = [nControl x nGrid] = control vector
%   defects = [nState x nSegment] = defect matrix
%   pathCost = scalar cost for the path integral
%
% NOTES:
%   - nGrid = nSegment*nSubStep+1
%   - This function is usually called twice for each combination of
%   decision variables: once by the objective function and once by the
%   constraint function. To keep the code fast I cache the old values and
%   only recompute when the inputs change.
%

%%%% CODE OPTIMIZATION %%%%
%
% Prevents the same exact code from being called twice by caching the
% solution and reusing it when appropriate.
%
global RUNGE_KUTTA_t RUNGE_KUTTA_x RUNGE_KUTTA_u
global RUNGE_KUTTA_defects RUNGE_KUTTA_pathCost
global RUNGE_KUTTA_decVars
%
usePreviousValues = false;
if ~isempty(RUNGE_KUTTA_decVars)
    if length(RUNGE_KUTTA_decVars) == length(decVars)
        if ~any(RUNGE_KUTTA_decVars ~= decVars)
            usePreviousValues = true;
        end
    end
end
%
if usePreviousValues
    t = RUNGE_KUTTA_t;
    x = RUNGE_KUTTA_x;
    u = RUNGE_KUTTA_u;
    defects = RUNGE_KUTTA_defects;
    pathCost = RUNGE_KUTTA_pathCost;
else
    %
    %
    %%%% END CODE OPTIMIZATION %%%%

    [tSpan, state, control] = unPackDecVar(decVars,pack);

    nState = pack.nState;
    nSegment = pack.nSegment;
    nSubStep = pack.nSubStep;

    % NOTES:
    %   The following bit of code is a bit confusing, mostly due to the
    %   need for vectorization to make things run at a reasonable speed in
    %   Matlab. Part of the confusion comes because the decision variables
    %   include the state at the beginning of each segment, but the control
    %   at the beginning and middle of each substep - thus there are more
    %   control grid-points than state grid points. The calculations are
    %   vectorized over segments, but not sub-steps, since the result of
    %   one sub-step is required for the next.

    % time, state, and control at the ends of each substep
    nTime = 1+nSegment*nSubStep;
    t = linspace(tSpan(1), tSpan(2), nTime);
    x = zeros(nState, nTime);
    u = control(:,1:2:end); % Control a the endpoints of each segment
    uMid = control(:,2:2:end);  %Control at the mid-points of each segment
    c = zeros(1, nTime-1);  %Integral cost for each segment
    dt = (t(end)-t(1))/(nTime-1);

    idx = 1:nSubStep:(nTime-1);   %Indicies for the start of each segment
    x(:,[idx,end]) = state;   %Fill in the states that we already know

    for iSubStep = 1:nSubStep
        % March forward Runge-Kutta step

        t0 = t(idx);
        x0 = x(:,idx);

        k0 = combinedDynamics(t0,        x0,                         u(:,idx), dynFun,pathObj);
        k1 = combinedDynamics(t0+0.5*dt, x0 + 0.5*dt*k0(1:nState,:), uMid(:,idx), dynFun,pathObj);
        k2 = combinedDynamics(t0+0.5*dt, x0 + 0.5*dt*k1(1:nState,:), uMid(:,idx), dynFun,pathObj);
        k3 = combinedDynamics(t0+dt,     x0 +     dt*k2(1:nState,:), u(:,idx+1), dynFun,pathObj);
        z = (dt/6)*(k0 + 2*k1 + 2*k2 + k3);  %Change over the sub-step

        xNext = x0 + z(1:nState,:);  %Next state
        c(idx) = z(end,:);  %Integral of the cost function over this step

        if iSubStep == nSubStep %We've reached the end of the interval
            % Compute the defect vector:
            defects = xNext - x(:,idx+1);
        else
            % Store the state for next step in time
            idx = idx+1;   %  <-- This is important!!
            x(:,idx) = xNext;
        end

    end

    pathCost = sum(c);  %Sum up the integral cost over each segment

    %%%% Cache results to use on the next call to this function.
    RUNGE_KUTTA_t = t;
    RUNGE_KUTTA_x = x;
    RUNGE_KUTTA_u = u;
    RUNGE_KUTTA_defects = defects;
    RUNGE_KUTTA_pathCost = pathCost;

end

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function dz = combinedDynamics(t,x,u,dynFun,pathObj)
% dz = combinedDynamics(t,x,u,dynFun,pathObj)
%
% This function packages the dynamics and the cost function together so
% that they can be integrated at the same time.
%
% INPUTS:
%   t = [1, nTime] = time vector (grid points)
%   x = [nState, nTime] = state vector at each grid point
%   u = [nControl, nTime] = control vector at each grid point
%   dynamics(t,x,u) = dynamics function handle
%               dx = [nState, nTime] = dx/dt = derivative of state wrt time
%   pathObj(t,x,u) = integral cost function handle
%                 dObj = [1, nTime] = integrand from the cost function
%
% OUTPUTS:
%   dz = [dx; dObj] = combined dynamics of state and cost

dx = dynFun(t,x,u);
if isempty(pathObj)
    dc = zeros(size(t));
else
    dc = pathObj(t,x,u);
end

dz = [dx;dc];  %Combine and return

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%                 Analytic Gradient Stuff                           %%%%
%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function gradInfo = grad_computeInfo(pack)
%
% This function computes the matrix dimensions and indicies that are used
% to map the gradients from the user functions to the gradients needed by
% fmincon. The key difference is that the gradients in the user functions
% are with respect to their input (t,x,u) or (t0,x0,tF,xF), while the
% gradients for fmincon are with respect to all decision variables.
%
% INPUTS:
%   nDeVar = number of decision variables
%   pack = details about packing and unpacking the decision variables
%       .nTime
%       .nState
%       .nControl
%
% OUTPUTS:
%   gradInfo = details about how to transform gradients
%

%nTime = pack.nTime;
nState = pack.nState;
nGridState = pack.nGridState;
nControl = pack.nControl;
nGridControl = pack.nGridControl;

nDecVar = 2 + nState*nGridState + nControl*nGridControl;

zIdx = 1:nDecVar;
gradInfo.nDecVar = nDecVar;
[tIdx, xIdx, uIdx] = unPackDecVar(zIdx,pack);

gradInfo.tIdx = tIdx([1,end]);
gradInfo.xIdx = xIdx;
gradInfo.uIdx = uIdx;

nSegment = pack.nSegment;
nSubStep = pack.nSubStep;

% indices of decVars associated with u
indu = 1:2:(1+2*nSegment*nSubStep);
gradInfo.indu = uIdx(:,indu);
% indices of decVars associated with uMid
indumid = 2:2:(1+2*nSegment*nSubStep);
gradInfo.indumid = uIdx(:,indumid);

%%%% For unpacking the boundary constraints and objective:
gradInfo.bndIdxMap = [tIdx(1); xIdx(:,1); tIdx(end); xIdx(:,end)];

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [fail] = runGradientCheck(z_test, pack,dynamics, pathObj, bndObj, pathCst, bndCst, gradInfo)
%
% This function tests the analytic gradients of the objective and
% nonlinear constraints with the DERIVEST package. The finite difference
% calculations in matlab's optimization package were not sufficiently
% accurate.
%
GradientCheckTol = 1e-6;  %Analytic gradients must match numerical within this bound

fail = 0;

fprintf('\n%s\n','____________________________________________________________')
fprintf('%s\n','  DerivativeCheck Information with DERIVEST Package ')

% analytic gradient
[~, dcost] = myObjGrad(z_test, pack, dynamics, pathObj, bndObj, gradInfo);

% check gradient with derivest package
deriv = gradest(@(z) myObjGrad(z, pack, dynamics, pathObj, bndObj, gradInfo),z_test);

% print largest difference in numerical and analytic gradients
fprintf('\n%s\n','Objective function derivatives:')
fprintf('%s\n','Maximum relative difference between user-supplied')
fprintf('%s %1.5e \n','and finite-difference derivatives = ',max(abs(dcost-deriv')))
if any(abs(dcost-deriv') > GradientCheckTol)
    error('Objective gradient did not pass')
end

% analytic nonlinear constraints
[c, ceq,dc, dceq] = myCstGrad(z_test, pack, dynamics, pathObj, pathCst, bndCst, gradInfo);

% check nonlinear inequality constraints with 'jacobianest'
if ~isempty(c)
    jac = jacobianest(@(z) myConstraint(z, pack, dynamics, pathObj, pathCst, bndCst),z_test);

    % print largest difference in numerical and analytic gradients
    fprintf('\n%s\n','Nonlinear inequality constraint function derivatives:')
    fprintf('%s\n','Maximum relative difference between user-supplied')
    fprintf('%s %1.5e \n','and finite-difference derivatives = ',max(max(abs(dc-jac'))))
    if any(any(abs(dc - jac') > GradientCheckTol))
        error('Nonlinear inequality constraint did not pass')
    end
end

% check nonlinear equality constraints with 'jacobianest'
if ~isempty(ceq)
    jac = jacobianest(@(z) myCstGradCheckEq(z, pack, dynamics, pathObj, pathCst, bndCst),z_test);

    % print largest difference in numerical and analytic gradients
    fprintf('\n%s\n','Nonlinear equality constraint function derivatives:')
    fprintf('%s\n','Maximum relative difference between user-supplied')
    fprintf('%s %1.5e \n','and finite-difference derivatives = ',max(max(abs(dceq-jac'))))
    if any(any(abs(dceq - jac') > GradientCheckTol))
        error('Nonlinear equality constraint did not pass')
    end
end

fprintf('\n%s\n','DerivativeCheck successfully passed.')
fprintf('%s\n','____________________________________________________________')
end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function ceq = myCstGradCheckEq(decVars, pack, dynamics, pathObj, pathCst, bndCst)
% This function is necessary for runGradientCheck function
% return only equality constraint (ceq) for use with jacobest.m

[t,x,u,defects] = simulateSystem(decVars, pack, dynamics, pathObj);

%%%% Call user-defined constraints and pack up:
[~, ceq] = collectConstraints(t,x,u,...
    defects,...
    pathCst, bndCst);

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [cost, dcost] = myObjGrad(decVars, pack,dynamics, pathObj, bndObj, gradInfo)
%
% This function unpacks the decision variables, sends them to the
% user-defined objective functions, and then returns the final cost
%
% INPUTS:
%   decVars = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynamics = user-defined dynamics function handle
%   pathObj = user-defined path-objective function
%   bndObj = user-defined boundary objective function
%   gradInfo =
%
% OUTPUTS:
%   cost = scalar cost for this set of decision variables
%   dcost = gradient of cost
%     NOTE: gradients are only available for pathCost that depends only on
%     input parameters not states.
%
%

% All of the real work happens inside this function:
[t,x,~,~,pathCost,dxdalpha,dJdalpha] = simSysGrad(decVars, pack, dynamics, pathObj, gradInfo); %#ok<ASGLU>
% dxdalpha is included in outputs to make sure subsequent calls to
% simulateSystem without change a to decVars have access to the correct value
% of dxdalpha - see simulateSystem in which dxdalpha is not calculated unless
% nargout > 5

% Compute the cost at the boundaries of the trajectory
if isempty(bndObj)
    bndCost = 0;
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    bndCost = bndObj(t0,x0,tF,xF);
end

cost = pathCost + bndCost;

% calculate gradient of cost function
if nargout > 1

    nState = pack.nState;
    nControl = pack.nControl;
    nSegment = pack.nSegment;
    nSubStep = pack.nSubStep;
    nDecVar = 2+nState*(1+nSegment)+nControl*(1+nSegment*nSubStep*2);

    % allocate gradient of cost
    dcost_pth = zeros(nDecVar,1);
    dcost_bnd = zeros(nDecVar,1);

    % gradient assocated with bound objective
    if ~isempty(bndObj)

        % bound costs and gradients w.r.t. t0, x0, tF, xF
        [~, d_bnd] = bndObj(t0,x0,tF,xF);

        % gradients of t0, x0, tF, xF w.r.t. decision parameters (labeled alpha)
        dt0_dalpha = zeros(1,nDecVar);
        dt0_dalpha(1) = 1; % t0 is always the first decVar
        %
        dx0_dalpha = zeros(nState,nDecVar);
        dx0_dalpha(1:nState,gradInfo.xIdx(:,end)) = eye(nState);
        %
        dtF_dalpha = zeros(1,nDecVar);
        dtF_dalpha(2) = 1; % tF is always the second decVar
        %
        dxF_dalpha = zeros(nState,nDecVar);
        dxF_dalpha(1:nState,gradInfo.xIdx(:,end)) = eye(nState);

        % gradient of bound cost
        dcost_bnd(:) = [dt0_dalpha; dx0_dalpha; dtF_dalpha; dxF_dalpha]' * d_bnd';
    end

    % gradient assocated with path objective
    if ~isempty(pathObj)

        dcost_pth = dJdalpha';

    end

    dcost = dcost_pth + dcost_bnd;

end

end

function [c, ceq, dc, dceq] = myCstGrad(decVars, pack, dynamics, pathObj, pathCst, bndCst, gradInfo)
%
% This function unpacks the decision variables, computes the defects along
% the trajectory, and then evaluates the user-defined constraint functions.
%
% INPUTS:
%   decVars = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynamics = user-defined dynamics function handle
%   pathObj = user-defined path-objective function
%   pathCst = user-defined path-constraint function
%   bndCst = user-defined boundary constraint function
%   gradInfo =
%
% OUTPUTS:
%   c = non-linear inequality constraint
%   ceq = non-linear equatlity cosntraint
%   dc = gradient of c w.r.t. decVars
%   dceq = gradient of ceq w.r.t. decVars
%
% NOTE:
%   - path constraints are  satisfied at the start and end of each sub-step
%

[t,x,u,defects,pathcost,dxdalpha] = simSysGrad(decVars, pack, dynamics, pathObj, gradInfo); %#ok<ASGLU>

%%%% Call user-defined constraints and pack up:
if nargout <= 2
    [c, ceq] = collectConstraints(t,x,u,...
        defects,...
        pathCst, bndCst);

else

    [c, ceq, dc, dceq] = collectConstraintsGrad(t,x,u,...
        defects,...
        pathCst, bndCst, pack, gradInfo, dxdalpha);

end

end

function [c, ceq, dc, dceq] = collectConstraintsGrad(t,x,u,defects, pathCst, bndCst, pack, gradInfo, dxdalpha)
% [c, ceq, dc, dceq] = collectConstraints(t,x,u,defects, pathCst, bndCst, pack, gradInfo, dxdalpha)
%
% OptimTraj utility function.
%
% Collects the defects, calls user-defined constraints, and then packs
% everything up into a form that is good for fmincon.
%
% INPUTS:
%   t = time vector (time at each substep) nTime = 1+nSegment*nSubStep
%   x = state matrix (states at each time in t)
%   u = control matrix (control at each time in t)
%   defects = defects matrix
%   pathCst = user-defined path constraint function
%   bndCst = user-defined boundary constraint function
%   pack =
%   gradInfo =
%   dxdalpha = partial derivative of state at each substep w.r.t. decVars
%
% OUTPUTS:
%   c = inequality constraint for fmincon
%   ceq = equality constraint for fmincon
%   dc = gradient of c w.r.t. decVars
%   dceq = gradient of ceq w.r.t. decVars
%

% problem dimensions
nState = pack.nState;
nControl = pack.nControl;
nSegment = pack.nSegment;
nSubStep = pack.nSubStep;
nDecVar = 2+nState*(1+nSegment)+nControl*(1+nSegment*nSubStep*2);

%%%% defect constraints
ceq_dyn = reshape(defects,numel(defects),1);

dceq_dyn = zeros(nDecVar,length(ceq_dyn));
Inx = eye(nState);
for j = 1:nSegment
    rows = gradInfo.xIdx(:,j+1);
    cols = (j-1)*nState+(1:nState);
    dceq_dyn(:,cols) = dxdalpha{j}(:,:,end)';  % gradient w.r.t. to x_i(+)
    dceq_dyn(rows,cols) = -Inx; % gradient w.r.t. to x_i
end

%%%% Compute the user-defined constraints:

%%%% path constraints
if isempty(pathCst)
    c_path = [];
    ceq_path = [];
    dc_path = [];
    dceq_path = [];
else
    [c_pathRaw, ceq_pathRaw, c_pathGradRaw, ceq_pathGradRaw] = pathCst(t,x,u);
    c_path = reshape(c_pathRaw,numel(c_pathRaw),1);
    ceq_path = reshape(ceq_pathRaw,numel(ceq_pathRaw),1);

    dc_path = zeros(nDecVar,length(c_path));
    dceq_path = zeros(nDecVar,length(ceq_path));

    % dt/dalpha : gradient of time w.r.t. decVars
    dt_dalpha = zeros(1,nDecVar);
    nTime = 1+nSegment*nSubStep;
    n_time = 0:nTime-1;

    % gradients of path constraints
    nc = size(c_pathRaw,1); % number path constraints at each time
    nceq = size(ceq_pathRaw,1);
    for j = 1:(nSegment+1)
        for i = 1:nSubStep

            % d(t[n])/dalpha
            n_time0 = n_time((j-1)*nSubStep+i);
            dt_dalpha(1) = (1 - n_time0/(nTime-1));
            dt_dalpha(2) = (n_time0/(nTime-1));

            %
            if j < nSegment+1
                dxi_dalpha = dxdalpha{j}(:,:,i);
            else
                dxi_dalpha = zeros(nState,nDecVar);
                cols = gradInfo.xIdx(:,j);
                dxi_dalpha(:,cols) = eye(nState);
            end

            %
            dui_dalpha = zeros(nControl,nDecVar);
            cols = gradInfo.indu(:,(j-1)*nSubStep+i);
            dui_dalpha(:,cols) = eye(nControl);

            % inequality path constraints
            if nc > 0
                cols = (1:nc) + nc*((j-1)*nSubStep+i-1);
                dc_path(:,cols) = [dt_dalpha; dxi_dalpha; dui_dalpha]' * c_pathGradRaw(:,:,nSubStep*(j-1)+i)';
            end

            % equality path constraints
            if nceq > 0
                cols = (1:nceq) + nceq*((j-1)*nSubStep+i-1);
                dceq_path(:,cols) = [dt_dalpha; dxi_dalpha; dui_dalpha]' * ceq_pathGradRaw(:,:,nSubStep*(j-1)+i)';
            end

            % no need to continue with inner loop.
            if j == nSegment+1
                break;
            end
        end
    end

end

%%%% bound constraints
if isempty(bndCst)
    c_bnd = [];
    ceq_bnd = [];
    dc_bnd = [];
    dceq_bnd = [];

else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);

    % bound constraints and gradients w.r.t. t0, x0, tF, xF
    [c_bnd, ceq_bnd, d_bnd, deq_bnd] = bndCst(t0,x0,tF,xF);

    % gradients of t0, x0, tF, xF w.r.t. decision parameters (labeled alpha)
    dt0_dalpha = zeros(1,nDecVar);
    dt0_dalpha(1) = 1; % t0 is always the first decVar
    %
    dx0_dalpha = zeros(nState,nDecVar);
    cols = gradInfo.xIdx(:,1);
    dx0_dalpha(1:nState,cols) = eye(nState);
    %
    dtF_dalpha = zeros(1,nDecVar);
    dtF_dalpha(2) = 1; % tF is always the second decVar
    %
    dxF_dalpha = zeros(nState,nDecVar);
    cols = gradInfo.xIdx(:,end);
    dxF_dalpha(1:nState,cols) = eye(nState);

    % inequality bound constraints
    dc_bnd = [dt0_dalpha; dx0_dalpha; dtF_dalpha; dxF_dalpha]' * d_bnd';

    % equality bound constraints
    dceq_bnd = [dt0_dalpha; dx0_dalpha; dtF_dalpha; dxF_dalpha]' * deq_bnd';

end

%%%% Pack everything up:
c = [c_path;c_bnd];
ceq = [ceq_dyn; ceq_path; ceq_bnd];

dc = [dc_path, dc_bnd];
dceq = [dceq_dyn, dceq_path, dceq_bnd];

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [t,x,u,defects,pathCost,dxdalpha,dJdalpha] = simSysGrad(decVars, pack, dynFun, pathObj, gradInfo)
%
% This function does the real work of the transcription method. It
% simulates the system forward in time across each segment of the
% trajectory, computes the integral of the cost function, and then matches
% up the defects between the end of each segment and the start of the next.
%
% INPUTS:
%   decVars = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynamics = user-defined dynamics function handle
%   pathObj = user-defined path-objective function
%
% OUTPUTS:
%   t = [1 x nGrid] = time vector for the edges of the sub-step grid
%   x = [nState x nGrid] = state vector
%   u = [nControl x nGrid] = control vector
%   defects = [nState x nSegment] = defect matrix
%   pathCost = scalar cost for the path integral
%
% NOTES:
%   - nGrid = nSegment*nSubStep+1
%   - This function is usually called twice for each combination of
%   decision variables: once by the objective function and once by the
%   constraint function. To keep the code fast I cache the old values and
%   only recompute when the inputs change.
%

%%%% CODE OPTIMIZATION %%%%
%
% Prevents the same exact code from being called twice by caching the
% solution and reusing it when appropriate.
%
global RUNGE_KUTTA_t RUNGE_KUTTA_x RUNGE_KUTTA_u
global RUNGE_KUTTA_defects RUNGE_KUTTA_pathCost
global RUNGE_KUTTA_decVars RUNGE_KUTTA_dxdalpha RUNGE_KUTTA_dJdalpha
%
usePreviousValues = false;
if ~isempty(RUNGE_KUTTA_decVars)
    if length(RUNGE_KUTTA_decVars) == length(decVars)
        if ~any(RUNGE_KUTTA_decVars ~= decVars)
            usePreviousValues = true;
        end
    end
end
%
if usePreviousValues
    t = RUNGE_KUTTA_t;
    x = RUNGE_KUTTA_x;
    u = RUNGE_KUTTA_u;
    defects = RUNGE_KUTTA_defects;
    pathCost = RUNGE_KUTTA_pathCost;
    dxdalpha = RUNGE_KUTTA_dxdalpha;
    dJdalpha = RUNGE_KUTTA_dJdalpha;
else
    %
    %
    %%%% END CODE OPTIMIZATION %%%%

    [tSpan, state, control] = unPackDecVar(decVars,pack);

    nState = pack.nState;
    nControl = pack.nControl;
    nSegment = pack.nSegment;
    nSubStep = pack.nSubStep;

    % NOTES:
    %   The following bit of code is a bit confusing, mostly due to the
    %   need for vectorization to make things run at a reasonable speed in
    %   Matlab. Part of the confusion comes because the decision variables
    %   include the state at the beginning of each segment, but the control
    %   at the beginning and middle of each substep - thus there are more
    %   control grid-points than state grid points. The calculations are
    %   vectorized over segments, but not sub-steps, since the result of
    %   one sub-step is required for the next.

    % time, state, and control at the ends of each substep
    nTime = 1+nSegment*nSubStep;
    t = linspace(tSpan(1), tSpan(2), nTime);
    x = zeros(nState, nTime);
    u = control(:,1:2:end); % Control a the endpoints of each segment
    uMid = control(:,2:2:end);  %Control at the mid-points of each segment
    c = zeros(1, nTime-1);  %Integral cost for each segment
    dt = (t(end)-t(1))/(nTime-1);

    idx = 1:nSubStep:(nTime-1);   %Indicies for the start of each segment
    x(:,[idx,end]) = state;   %Fill in the states that we already know

    % VARIABLES for analytic gradient evaluations.
    % size of decicion parameters (2 for time), nstate*(nSegment+1), ...
    % dxdalpha = partial derivative of state w.r.t. decVars (alpha)
    nalpha = 2 + nState*(1+nSegment) + nControl*(1+2*nSubStep*nSegment);
    dxdalpha = cell(1,nSegment);
    for i = 1:nSegment
        dxdalpha{i} = zeros(nState,nalpha,nSubStep+1);
        cols = gradInfo.xIdx(:,i);
        dxdalpha{i}(:,cols,1) = eye(nState);
    end
    dTdalpha = zeros(1,nalpha); dTdalpha(1:2) = [-1,1];
    dt_dalpha = zeros(1,nalpha);
    n_time = 0:nTime-1;

    % gradient of path cost
    dJdalpha = zeros(1,nalpha);

    for iSubStep = 1:nSubStep
        % March forward Runge-Kutta step

        t0 = t(idx);
        x0 = x(:,idx);

        %------------------------------------------
        % Code for calculating dxdalpha (partial derivative of state w.r.t.
        % the descision parameters): dxdalpha = nstate x nalpha
        % assume nargout <=5 when using finite difference calculation for
        % gradients in which case dxdalpha is unnecessary.

        % Gradient of time w.r.t. decVars
        % ------------------------------------------------------------
        % dt = (tF-t0)/(nTime-1)
        % t = t0 + n*dt
        % t = t0 + n*(tF-t0)/(nTime-1)
        % t = t0*(1-n/(nTime-1)) + tF*(n/(nTime-1))
        %
        % alpha = [t0, tF, x0, x1, ..., xN, u0, uM0, u1, ..., uN]
        % dt/dalpha = [1 - n/(nTime-1), n/(nTime-1), 0, 0, ... 0]
        % ------------------------------------------------------------

        n_time0 = n_time(idx);

        [k0, dk0] = combinedDynGrad(t0,        x0,                         u(:,idx), dynFun,pathObj);
        [k1, dk1] = combinedDynGrad(t0+0.5*dt, x0 + 0.5*dt*k0(1:nState,:), uMid(:,idx), dynFun,pathObj);
        [k2, dk2] = combinedDynGrad(t0+0.5*dt, x0 + 0.5*dt*k1(1:nState,:), uMid(:,idx), dynFun,pathObj);
        [k3, dk3] = combinedDynGrad(t0+dt,     x0 +     dt*k2(1:nState,:), u(:,idx+1), dynFun,pathObj);
        z = (dt/6)*(k0 + 2*k1 + 2*k2 + k3);  %Change over the sub-step

        for j = 1:nSegment

            % d(t[n])/dalpha
            dt_dalpha(1) = (1 - n_time0(j)/(nTime-1));
            dt_dalpha(2) = (n_time0(j)/(nTime-1));

            % du[n]/dalpha
            du_dalpha = zeros(nControl,nalpha);
            du_dalpha(:,gradInfo.indu(:,idx(j))) = eye(nControl);

            % duMid[n]/dalpha
            duMid_dalpha = zeros(nControl,nalpha);
            duMid_dalpha(:,gradInfo.indumid(:,idx(j))) = eye(nControl);

            % du[n+1]/dalpha
            du1_dalpha = zeros(nControl,nalpha);
            du1_dalpha(:,gradInfo.indu(:,idx(j)+1)) = eye(nControl);

            % dk0/dalpha
            dk0da = dk0(:,:,j) * [dt_dalpha; dxdalpha{j}(:,:,iSubStep); du_dalpha];

            % dk1/dalpha
            dk1da = dk1(:,:,j) * [dt_dalpha + 0.5/(nTime-1)*dTdalpha; dxdalpha{j}(:,:,iSubStep) + 0.5*dt*dk0da(1:nState,:) + 0.5/(nTime-1)*k0(1:nState,j)*dTdalpha; duMid_dalpha];

            % dk2/dalpha
            dk2da = dk2(:,:,j) * [dt_dalpha + 0.5/(nTime-1)*dTdalpha; dxdalpha{j}(:,:,iSubStep) + 0.5*dt*dk1da(1:nState,:) + 0.5/(nTime-1)*k1(1:nState,j)*dTdalpha; duMid_dalpha];

            % dk3/dalpha
            dk3da = dk3(:,:,j) * [dt_dalpha + 1/(nTime-1)*dTdalpha; dxdalpha{j}(:,:,iSubStep) + dt*dk2da(1:nState,:) + 1/(nTime-1)*k2(1:nState,j)*dTdalpha; du1_dalpha];

            dz = (dt/6)*(dk0da + 2*dk1da + 2*dk2da + dk3da)...
                + 1/(6*(nTime-1))*(k0(:,j)+2*k1(:,j)+2*k2(:,j)+k3(:,j))*dTdalpha;

            % update dxdalpha
            dxdalpha{j}(:,:,iSubStep+1) = dxdalpha{j}(:,:,iSubStep) + dz(1:nState,:);

            % update dJdalpha
            dJdalpha  = dJdalpha + dz(nState+1,:);
        end

        xNext = x0 + z(1:nState,:);  %Next state
        c(idx) = z(end,:);  %Integral of the cost function over this step

        if iSubStep == nSubStep %We've reached the end of the interval
            % Compute the defect vector:
            defects = xNext - x(:,idx+1);
        else
            % Store the state for next step in time
            idx = idx+1;   %  <-- This is important!!
            x(:,idx) = xNext;
        end

    end

    pathCost = sum(c);  %Sum up the integral cost over each segment

    %%%% Cache results to use on the next call to this function.
    RUNGE_KUTTA_t = t;
    RUNGE_KUTTA_x = x;
    RUNGE_KUTTA_u = u;
    RUNGE_KUTTA_defects = defects;
    RUNGE_KUTTA_pathCost = pathCost;
    RUNGE_KUTTA_dxdalpha = dxdalpha;
    RUNGE_KUTTA_dJdalpha = dJdalpha;

end

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [dz, J] = combinedDynGrad(t,x,u,dynFun,pathObj)
% [dz, dJ] = combinedDynGrad(t,x,u,dynFun,pathObj)
%
% This function packages the dynamics and the cost function together so
% that they can be integrated at the same time.
%
% INPUTS:
%   t = [1, nTime] = time vector (grid points)
%   x = [nState, nTime] = state vector at each grid point
%   u = [nControl, nTime] = control vector at each grid point
%   dynamics(t,x,u) = dynamics function handle
%               dx = [nState, nTime] = dx/dt = derivative of state wrt time
%   pathObj(t,x,u) = integral cost function handle
%                 dObj = [1, nTime] = integrand from the cost function
%
% OUTPUTS:
%   dz = [dx; dObj] = combined dynamics of state and cost
%   dJ = [JAC(dynamics), JAC(objective)] = combined jacobian of dynamics
%   and objective w.r.t. (t,x,u)

nState = size(x,1);
nControl = size(u,1);

[dx,Jx] = dynFun(t,x,u);
if isempty(pathObj)
    dc = zeros(size(t));
    Jc = zeros(1,1+nState+nControl,length(t));
else
    [dc,Jc] = pathObj(t,x,u);
    Jc = reshape(Jc,1,1+nState+nControl,length(t));
end

dz = [dx;dc];

J = cat(1,Jx,Jc);

end

author: MatthewPeterKelly

repo: OptimTraj


text:
function mrAnatSetNiftiXform(niftiFile, outFile);
%
% mrAnatSetNiftiXform([niftiFile=uigetfile],[outFile=uiputfile])
%
% Allows you to set the qto xform in a nifti file.
%
% REQUIRES:
%  * Stanford anatomy tools (eg. /usr/local/matlab/toolbox/mri/Anatomy)
%
% HISTORY:
% 2006.10.25 RFD (bob@white.stanford.edu) wrote it.

if (~exist('niftiFile','var') || isempty(niftiFile))
    [f,p] = uigetfile({'*.nii.gz','NIFTI';'*.*', 'All Files (*.*)'}, 'Select NIFTI file...');
    if(isnumeric(f)) disp('User canceled.'); return; end
    niftiFile = fullfile(p,f);
end

ni = niftiRead(niftiFile);
ni = niftiApplyCannonicalXform(ni);
img = mrAnatHistogramClip(double(ni.data), 0.4, 0.98);

nii.img = img;
nii.hdr.dime.pixdim = [1 ni.pixdim 1 1 1 1];
nii.hdr.dime.datatype = 64;
nii.hdr.dime.dim = [3 size(nii.img) 1 1 1 1];
%nii.hdr.hist.originator = [round(ni.qto_ijk(1:3,:)*[0 0 0 1]')'+1 128 0];
nii.hdr.hist.originator = [round(size(nii.img)./2) 128 0];
h = figure('unit','normal','pos', [0.18 0.08 0.5 0.85],'name','Set AC-PC landmarks');
opt.setarea = [0.05 0.15 0.9 0.8];
opt.usecolorbar = 0;
opt.usestretch = 0;
opt.command = 'init';
view_nii(h, nii, opt);
%d = getappdata(h);
hstr = num2str(h);
cb = ['d=getappdata(' hstr ');p=d.nii_view.imgXYZ.vox;setappdata(' hstr ',''ac'',p);set(gcbo,''String'',[''AC=['' num2str(p) '']'']);'];
b1 = uicontrol(h, 'Style','pushbutton','Visible','on','String','Set AC','Position',[20 30 150 30],'Callback',cb);
cb = ['d=getappdata(' hstr ');p=d.nii_view.imgXYZ.vox;setappdata(' hstr ',''pc'',p);set(gcbo,''String'',[''PC=['' num2str(p) '']'']);'];
b2 = uicontrol(h, 'Style','pushbutton','Visible','on','String','Set PC','Position',[190 30 150 30],'Callback',cb);
cb = ['d=getappdata(' hstr ');p=d.nii_view.imgXYZ.vox;setappdata(' hstr ',''ms'',p);set(gcbo,''String'',[''MidSag=['' num2str(p) '']'']);'];
b3 = uicontrol(h, 'Style','pushbutton','Visible','on','String','Set MidSag','Position',[360 30 150 30],'Callback',cb);
cb = ['setappdata(' hstr ',''done'',1);'];
b4 = uicontrol(h, 'Style','pushbutton','Visible','on','String','FINISH','Position',[530 30 80 30],'Callback',cb);
done = false;
while(~done)
    d = getappdata(h);
    if(isfield(d,'ac')&&isfield(d,'pc')&&isfield(d,'ms')&&isfield(d,'done')&&d.done==1)
        done = true;
        %convert matlab 1-based indices to zero-indexed indices
        %alignLandmarks = [d.ac; d.pc; d.ms]-1;
        alignLandmarks = [d.ac; d.pc; d.ms];
    end
    pause(.1);
end
close(h);

%disp(alignLandmarks);
origin = alignLandmarks(1,:);
% Define the current image axes by re-centering on the origin (the AC)
imY = alignLandmarks(2,:)-origin; imY = imY./norm(imY);
imZ = alignLandmarks(3,:)-origin; imZ = imZ./norm(imZ);
imX = cross(imZ,imY);
% Make sure the vectors point right, superior, anterior
if(imX(1)<0) imX = -imX; end
if(imY(2)<0) imY = -imY; end
if(imZ(3)<0) imZ = -imZ; end
% Project the current image axes to the cannonical AC-PC axes. These
% are defined as X=[1,0,0], Y=[0,1,0], Z=[0,0,1], with the origin
% (0,0,0) at the AC. Note that the following are the projections
x = [0 1 imY(3)]; x = x./norm(x);
y = [1  0 imX(3)]; y = y./norm(y);
%z = [0  imX(2) 1]; z = z./norm(z);
z = [0  -imY(1) 1]; z = z./norm(z);
% Define the 3 rotations using the projections. We have to set the sign
% of the rotation, depending on which side of the plane we came from.
rot(1) = sign(x(3))*acos(dot(x,[0 1 0])); % rot about x-axis (pitch)
rot(2) = sign(y(3))*acos(dot(y,[1 0 0])); % rot about y-axis (roll)
rot(3) = sign(z(2))*acos(dot(z,[0 0 1])); % rot about z-axis (yaw)

scale = ni.pixdim;
% Affine build assumes that we need to translate before rotating. But,
% our rotations have been computed about the origin, so we'll pass a
% zero translation and set it ourselves (below).
im2tal = affineBuild([0 0 0], rot, scale, [0 0 0]);
tal2im = inv(im2tal);

% Insert the translation.
%tal2im(1:3,4) = [origin+scale/2]';
tal2im(1:3,4) = [origin]';
im2tal = inv(tal2im);

if (~exist('outFile','var') || isempty(outFile))
    outFile = niftiFile;
end
resp = questdlg(['Save new transform matrix in ' outFile '?'],'Save confirmation','Ok','Cancel','Ok');
if(strcmpi(resp,'cancel'))
    disp('user canceled- transform NOT saved.');
    return;
end
if(exist(outFile,'file'))
    clear ni;
    ni = niftiRead(outFile);
    ni = niftiApplyCannonicalXform(ni);
else
    ni.fname = outFile;
end
ni = niftiSetQto(ni,im2tal,true);
% NOTE: our data are always left-handed (ie. 'neurological;
% unflipped; left-is-left). So, we force the qfac to reflect
% that.
ni.qfac = 1;

writeFileNifti(ni);
disp('transform saved.');
return;

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
function [logp, yhat, res] = tapas_softmax_2beta(r, infStates, ptrans)
% Calculates the log-probability of responses under the softmax model with different betas for
% rewards and punishments
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Predictions or posteriors?
predorpost = r.c_obs.predorpost;

% Transform betas to their native space
% be(1): rewards, be(2): punishments
be = exp(ptrans(1:2));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from inferred states, inputs, and responses
states = squeeze(infStates(:,1,:,1,predorpost));
states(r.irr,:) = [];
u = r.u(:,1);
u(r.irr) = [];
y = r.y(:,1);
y(r.irr) = [];

% Number of choices
nc = size(infStates,3);

% Partition functions
Z1 = sum(exp(be(1)*states),2);
Z1 = repmat(Z1,1,nc);

Z2 = sum(exp(be(2)*states),2);
Z2 = repmat(Z2,1,nc);

% Softmax probabilities
prob1 = exp(be(1)*states)./Z1;
prob2 = exp(be(2)*states)./Z2;

% Extract probabilities of chosen options
probc1 = prob1(sub2ind(size(prob1), 1:length(y), y'));
probc2 = prob2(sub2ind(size(prob2), 1:length(y), y'));

% Choose the correct column
probc = probc1'.*(u==1) +probc2'.*(u==0);

% Calculate log-probabilities for non-irregular trials
reg = ~ismember(1:n,r.irr);
logp(reg) = log(probc);
yhat(reg) = probc;
res(reg) = -log(probc);

return;

author: translationalneuromodeling

repo: tapas


text:
%% processing for fluxes
%find the maximal set of metabolites and reactions that are stoichiometrically consistent
if ~isfield(model,'SConsistentMetBool') || ~isfield(model,'SConsistentRxnBool')
    massBalanceCheck=0;
    [~, ~, ~, ~, ~, ~, model, ~] = findStoichConsistentSubset(model, massBalanceCheck, param.printLevel-1);
end

if ~isfield(model,'osenseStr') || isempty(model.osenseStr)
    %default linear objective sense is maximisation
    model.osenseStr = 'max';
end
[~,osense] = getObjectiveSense(model);

if ~isfield(param,'maxUnidirectionalFlux')
    %try to set the maximum unidirectional flux based on the magnitude of the largest bound but dont have it greater than 1e5
    %param.maxUnidirectionalFlux=min(1e5,max(abs(model.ub)));
    param.maxUnidirectionalFlux=inf;
end
if ~isfield(param,'minUnidirectionalFlux')
    if isequal(param.solver,'mosek')
        %try to set the minimum unidirectional flux
        param.minUnidirectionalFlux=0;
    else
        param.minUnidirectionalFlux = 0;
    end
end

if ~isfield(param,'internalNetFluxBounds')
    param.internalNetFluxBounds='original';
end
if isfield(param,'internalBounds')
    error('internalBounds replaced by other parameter options')
end

if param.debug
    solution_optimizeCbModel = optimizeCbModel(model);
    switch solution_optimizeCbModel.stat
        case 0
            message = 'Input model is not feasible according to optimizeCbModel.';
            warning(message)
            solution = solution_optimizeCbModel;
            modelOut = model;
            return
        case 1
            message ='Input model is feasible according to optimizeCbModel.';
            disp(message)
            solution = solution_optimizeCbModel;
            messages = cellstr(message);
    end
end

if 0
    %ignore this for now - TODO
    nMetInRxn=sum(model.S~=0,1);
    if any(nMetInRxn==0)
        nTrivialRxn = nnz(nMetInRxn==0);
        error(['model.S is incorrectly specified as it contains ' int2str(nTrivialRxn) ' zero columns'])
    end

    nonUnitaryRxn = (nMetInRxn~=1)';
    rxnBool = nonUnitaryRxn & ~model.SConsistentRxnBool;
    if any(rxnBool)
        if param.printLevel> 0
            fprintf('%s\n',['Assigned ' int2str(nnz(rxnBool)) ' non-unitary exchange reactions as stoichiometrically consistent.'])
            if param.printLevel> 1
                printConstraints(model, -inf, inf, rxnBool, [], 0);
            end
        end
        %non-unitary exchange reactions are assumed to be stoichiometrically
        %consistent because it is assumed this model was generated using
        %thermoKernel, and all reactions should be forced in some way
        %TODO why is this reaction one:
        %'CYSTS_H2S'        'Cystathionine Beta-Synthase (sulfide-forming)'    -10000    10000    'cyk_L[c] + hcyk_L[c]  <=> HC00250[c] + cyst_L[c] '
        model.SConsistentRxnBool = model.SConsistentRxnBool | rxnBool;
    end
end

lb=model.lb;
ub=model.ub;
switch param.internalNetFluxBounds
    case {'originalNetFluxBounds','original'}
        if param.printLevel>0
            fprintf('%s\n','Using existing internal net flux bounds without modification.')
        end
    case 'directional'
        if param.printLevel>0
            fprintf('%s\n','Using directional internal net flux bounds only.')
        end
        lb(lb<0 & model.SConsistentRxnBool,1)=-param.maxUnidirectionalFlux;
        lb(lb>0 & model.SConsistentRxnBool,1)=0;

        ub(ub>0 & model.SConsistentRxnBool,1)=param.maxUnidirectionalFlux;
        ub(ub<0 & model.SConsistentRxnBool,1)=0;
    case {'maxInternalNetFluxBounds','max'}
        lb(model.SConsistentRxnBool)=-ones(n,1)*param.maxUnidirectionalFlux;
        ub(model.SConsistentRxnBool)= ones(n,1)*param.maxUnidirectionalFlux;

    case 'none'
        if param.printLevel>0
            fprintf('%s\n','Using no internal net flux bounds.')
        end
        lb(model.SConsistentRxnBool,1)=-ones(n,1)*inf;
        ub(model.SConsistentRxnBool,1)= ones(n,1)*inf;

    case 'random'
        lb(model.SConsistentRxnBool,1)=-rand(n,1)*param.maxUnidirectionalFlux;
        ub(model.SConsistentRxnBool,1)= rand(n,1)*param.maxUnidirectionalFlux;

    case 'rangeNt'
        u1=rand(m,1);
        u2=rand(m,1);
        u=u2-u1;
        %reorient S
        N=N*diag(sign(N'*u));
        lb(model.SConsistentRxnBool,1)=N'*u2;
        ub(model.SConsistentRxnBool,1)=N'*u1;

    case 'expRangeNt'
        u1=rand(m,1);
        u2=rand(m,1);
        u=u2-u1;
        %reorient S
        N=N*diag(sign(N'*u));
        lb(model.SConsistentRxnBool,1)=-exp(N'*u2);
        ub(model.SConsistentRxnBool,1)= exp(N'*u1);

    otherwise
        error(['param.internalNetFluxBounds = ' param.internalNetFluxBounds ' is an unrecognised input'])
end
vl = lb(model.SConsistentRxnBool);
vu = ub(model.SConsistentRxnBool);
%exchange reaction bounds (may be overwritten if conc method is chosen)
vel = lb(~model.SConsistentRxnBool);
veu = ub(~model.SConsistentRxnBool);

relaxedUnidirectionalUpperBounds = 1;
%lower bound on forward fluxes
if isfield(model,'vfl')
    vfl = model.vfl;
else
    vfl = max(param.minUnidirectionalFlux,vl);
end
%upper bounds on forward fluxes
if isfield(model,'vfu')
    vfu = model.vfu;
else
    if relaxedUnidirectionalUpperBounds
        vfu = ones(n,1)*param.maxUnidirectionalFlux;
    else
        vfu = vu;
        vfu(vfu<=0) = param.maxUnidirectionalFlux;
    end
end
%lower bounds on reverse fluxes
if isfield(model,'vrl')
    vrl = model.vrl;
else
    vrl = max(param.minUnidirectionalFlux,-vu);
end
%upper bounds on reverse fluxes
if isfield(model,'vru')
    vru = model.vru;
else
    if relaxedUnidirectionalUpperBounds
        vru = ones(n,1)*param.maxUnidirectionalFlux;
    else
        vru = -vl;
        vru(vru<=0) = param.maxUnidirectionalFlux;
    end
end
if any(vfl<0)
    error('lower bound on forward flux cannot be less than zero')
end
if any(vrl<0)
    error('lower bound on reverse flux cannot be less than zero')
end
if any(vfl>vfu)
    error('lower bound on forward flux greater than upper bound')
end
if any(vrl>vru)
    error('lower bound on reverse flux greater than upper bound')
end

if any(vl>0 | vu<0) && ~strcmp(param.internalNetFluxBounds,'original')
    if param.printLevel> 2
        histogram([vl;vu])
        title('internal reaction bounds not directional')
    end
    warning('internal reaction bounds not directional')
end

if ~isfield(model,'c') || isempty(model.c)
    ci = zeros(n,1);
    ce = zeros(k,1);
else
    %osense is only used to changes the sense of the model.c part
    ci = osense*model.c(model.SConsistentRxnBool);
    ce = osense*model.c(~model.SConsistentRxnBool);
end

if ~isfield(model,'cf') || isempty(model.cf)
    model.cf='zero';
end
if ~isfield(model,'cr') || isempty(model.cr)
    model.cr='zero';
end
if ischar(model.cf) || ischar(model.cr)
    switch model.cf
        case 'rand'
            cf=N'*rand(m,1);
            cr=-cf;
        case 'one'
            cf=ones(n,1);
            cr=ones(n,1);
        case 'zero'
            cf=zeros(n,1);
            cr=zeros(n,1);
    end
else
    if length(model.cf)~=length(model.cr)
        error('model.cf and model.cr must have the same dimensions')
    end
    if length(model.cf)==size(model.S,2)
        cf = columnVector(model.cf(model.SConsistentRxnBool));
        cr = columnVector(model.cr(model.SConsistentRxnBool));
    else
        if length(model.cf)==1
            model.cf=ones(n,1)*model.cf;
            model.cr=ones(n,1)*model.cr;
        end
        if length(model.cf)~=nnz(model.SConsistentRxnBool)
            error('cf and cr must have the same dimension as nnz(model.SConsistentRxnBool) x 1')
        else
            cf = columnVector(model.cf);
            cr = columnVector(model.cr);
        end
    end
    if any(~isfinite([cf;cr]))
        error('cf and cr must all be finite')
    end
end

if ~isfield(model,'g') || isempty(model.g)
    if isequal(param.method,'fluxes')
        model.g='one';
    else
        model.g='two';
    end
end
if ischar(model.g)
    switch model.g
        case 'zero'
            g=zeros(n,1);
        case 'rand'
            g=rand(n,1);
        case 'one'
            g=ones(n,1);
        case 'two'
            g=ones(n,1)*2;
        otherwise
            error('unrecognised option for model.g')
    end
else
    if length(model.g)==size(model.S,2)
        g = columnVector(model.g(model.SConsistentRxnBool));
    else
        if length(model.g)==1
            model.g=ones(n,1)*model.g;
        end
        if length(model.g)~=nnz(model.SConsistentRxnBool)
            error('g and cf must have the same dimension as nnz(model.SConsistentRxnBool) x 1')
        else
            g = columnVector(model.g);
        end
    end
    if any(~isfinite(g))
        error('g must all be finite')
    end
    if length(g)~=length(cf)
        error('g and cf must have the same dimensions')
    end
end

author: opencobra

repo: cobratoolbox


text:
function [ res, jac ] = opt01_rj ( x, flag )

%*****************************************************************************80
%
%% OPT01_RJ evaluates RES and JAC for test case #1.
%
%  Discussion:
%
%    This example is discussed in Dennis and Schnabel, pages 100, 104 and 202.
%
%    Suggested starting value for X is ( 1, 1 ).
%
%    The optimizing value is
%
%      X* = ( 2, -1 ),
%
%    for which
%
%      RES(X*) = (0,0,0).
%
%  Modified:
%
%    05 January 2008
%
%  Author:
%
%    Jeff Borggaard,
%    Gene Cliff,
%    Virginia Tech.
%
%  Reference:
%
%    John Dennis, Robert Schnabel,
%    Numerical Methods for Unconstrained Optimization
%    and Nonlinear Equations,
%    SIAM, 1996,
%    ISBN13: 978-0-898713-64-0,
%    LC: QA402.5.D44.
%
%  Parameters:
%
%    Input, real X(2), the evaluation point.
%
%    Input, string FLAG, indicates what must be computed.
%    'f' means only the value of RES is needed,
%    'g' means only the value of JAC is needed,
%    'all' means RES and JAC are needed.
%    It is acceptable to behave as though FLAG was 'all'
%    on every call.
%
%    Output, real RES(3), the residual column vector.
%
%    Output, real JAC(3,2), the Jacobian matrix.
%
  n = length ( x );

  if ( n ~= 2 )
    fprintf ( '\n' );
    fprintf ( 'OPT01_RJ - Fatal error!\n' );
    fprintf ( '  The input vector X should have length 2.\n'),
    fprintf ( '  Instead, it has length = %d.\n', n );
    keyboard
  end
%
%  Formulated as a nonlinear system:
%
  res(1,1) = ( x(1) - 2 )^2;
  res(2,1) = ( x(1) - 2 ) * x(2);
  res(3,1) = ( x(2) + 1);

  jac(1,1) = 2 * ( x(1) - 2 );
  jac(1,2) = 0;

  jac(2,1) = x(2);
  jac(2,2) = x(1) - 2;

  jac(3,1) = 0;
  jac(3,2) = 1;

return

author: johannesgerer

repo: jburkardt-m


text:
function header=nifticreate(img,format)
%
%    header=nifticreate(img)
%       or
%    header=nifticreate(img,format)
%
%    Create a default NIfTI header
%
%    author: Qianqian Fang (q.fang <at> neu.edu)
%
%    input:
%        img: the image data matching the header
%        format: can only be 'nifti1'. can be ignored
%
%    output:
%        header: a struct that is byte-wise compatible with NIfTI-1
%
%    this file is part of JNIfTI specification: https://github.com/fangq/jnifti
%
%    License: Apache 2.0, see https://github.com/fangq/jnifti for details
%

if(nargin<2)
    format='nifti1';
end

datatype=struct('int8',256,'int16',4,'int32',8,'int64',1024,'uint8',2,'uint16',512,'uint32',768,'uint64',1280,'single',16,'double',64);

if(strcmp(format,'nifti1'))
    headerlen=348;
else
    headerlen=540;
end

header=memmapstream(uint8(zeros(1,headerlen+4)),niiformat(format));
header.sizeof_hdr=cast(headerlen, class(header.sizeof_hdr));
header.datatype=cast(datatype.(class(img)), class(header.datatype));
header.dim(1:end)=cast(1,class(header.dim));
header.dim(1:ndims(img)+1)=cast([ndims(img),size(img)], class(header.dim));
header.pixdim(1:ndims(img)+1)=cast(1, class(header.pixdim));
header.vox_offset=cast(headerlen+4, class(header.vox_offset));
if(header.sizeof_hdr==540)
    header.magic(1:3)=cast('ni2',class(header.magic));
else
    header.magic(1:3)=cast('ni1',class(header.magic));
end
header.srow_x(1)=cast(1, class(header.srow_x));
header.srow_y(2)=cast(1, class(header.srow_y));
header.srow_z(3)=cast(1, class(header.srow_z));
header.sform_code=cast(1, class(header.sform_code));

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs, non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

%RL = springs_info(:,4); % resting-length vector

% Contraction Frequency
freq = 1.0;

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(239:end,4) = abs( cos(freq*current_time*pi) );

%NOTE: not 2*pi*ft b/c of abs()

author: nickabattista


text:
%% Example
% real time control of the KUKA iiwa 7 R 800
% the impedence control is turned on
% Moving first joint of the robot, using a sinisoidal function

% An example script, it is used to show how to use the different
% functions of the KUKA Sunrise matlab toolbox

% First run the following script in Matlab
% Then start the client on the KUKA iiwa controller

% Mohammad SAFEEA, 24th of April 2018

close all,clear all;clc;

warning('off')

ip='172.31.1.147'; % The IP of the controller
% start a connection with the server
global t_Kuka;
t_Kuka=net_establishConnection( ip );

if ~exist('t_Kuka','var') || isempty(t_Kuka) || strcmp(t_Kuka.Status,'closed')
  warning('Connection could not be establised, script aborted');
  return;
else

      %% Get position roientation of end effector

%      jPos={0,pi/6,0,-pi/2,0,pi/2-pi/6,0};

        jPos={0,0,0,-pi/2,0,pi/2,0};
      %jPos={0,0,0,0,0,0,0};
      setBlueOff(t_Kuka); % turn Off blue light

      relVel=0.15;
      movePTPJointSpace( t_Kuka , jPos, relVel); % move to initial configuration
     %% Pause for 3 seocnds
     pause(3);
        %% Start direct servo in joint space
        massOfTool=0.5; % the mass of the tool attached to flange in Kg
        cOMx=0; % X coordinate of the center of mass of the tool in (mm)
        cOMy=0; % Y coordinate of the center of mass of the tool in (mm)
        cOMz=40; % Z coordinate of the center of mass of the tool in (mm)
        cStiness=900; % cartizian stifness
        rStifness=80; % rotational stifness
        nStifness=50; % null space stifness

        % Start the realtime control with impedence
        realTime_startImpedanceJoints(t_Kuka,massOfTool,cOMx,cOMy,cOMz,...
        cStiness,rStifness,nStifness);

       w=0.6; % motion constants, frequency rad/sec
       A=0.2; % motion constants, amplitude of motion

       a=datevec(now);
       t0=a(6)+a(5)*60+a(4)*60*60; % calculate initial time

       dt=0;
     precission=1000;
     precission_flag=true;
     tstart=t0;
     counter=0;
     duration=1*60; %1 minutes
     time_stamps=zeros(1,1000*duration);
       while(dt<duration)
         %% ferform trajectory calculation here
          a=datevec(now);
          time=a(6)+a(5)*60+a(4)*60*60;
          dt=time-t0;

          temp=A*(1-cos(w*dt));
            for dacount=1:7
                jPosCommand{dacount}=jPos{dacount}+temp;
            end
          counter=counter+1;
          %% Send joint positions to robot
          %sendJointsPositions( t ,jPos);
          sendJointsPositionsf( t_Kuka ,jPosCommand);
          time_stamps(counter)=dt;

       end
       tend=time;
       rate=counter/(tend-tstart);

       %% Stop the realtime control with impedence motion
       realTime_stopImpedanceJoints( t_Kuka );

       fprintf('\nThe rate of joint nagles update per second is: \n');
       disp(rate);
       fprintf('\n')
       pause(2);
       %% turn off light
       setBlueOff(t_Kuka);

      %% turn off the server
       net_turnOffServer( t_Kuka );

       fclose(t_Kuka);

       %% save time stamps
       save('timingdata.mat','time_stamps');

end
 warning('on')

repo: KST-Kuka-Sunrise-Toolbox


text:
function displayProbGraph(pathRF,nameOutcomes,nameSets,thesholds)
% nameOutcomes and nameSets are associated: one set per outcome

startpath = pwd;
nOutcomes = numel(nameOutcomes);
symbols = {'ob','xr'}; % First entry for positive instances, second entry for negative instances

figure
cd(pathRF), load('testing')
nameThick = cell(1,nOutcomes*2+3); nameThick{1} = '';
for o = 1:nOutcomes
    nameOutcome = nameOutcomes{o}; fSet = nameSets{o};
    %load(['testResultsRF_',fSet,'_',nameOutcome]) % results gets out of there
    load(['testResults_',fSet,'_',nameOutcome]) % results gets out of there
    probData = results.probResponse;
    outcome = testing.outcomes.(nameOutcome);
    probPos = probData(outcome == 1); xPos = ones(numel(probPos),1)*o;
    probNeg = probData(outcome == 0); xNeg = ones(numel(probNeg),1)*o;
    plot(xPos,probPos,symbols{1},'LineWidth',6,'MarkerSize',18,'MarkerFaceColor',symbols{1}(end),'MarkerEdgeColor',symbols{1}(end))
    hold on
    plot(xNeg,probNeg,symbols{2},'LineWidth',6,'MarkerSize',20,'MarkerFaceColor',symbols{2}(end),'MarkerEdgeColor',symbols{2}(end))
    hold on
    nameThick{o*2} = ''; nameThick{o*2+1} = [nameOutcome,'_{',nameSets{o},'+clinic}'];
end
xThres = (0+0.05):0.05:(nOutcomes+1-0.05);
plot(xThres,ones(1,numel(xThres))*thesholds(1)/100,'--m','LineWidth',4), hold on
plot(xThres,ones(1,numel(xThres))*thesholds(2)/100,'--m','LineWidth',4)
nameThick{end} = '';
title('Probability of occurence of events: testing cohorts','FontSize',36)
ylabel(['Random forest output probability'],'FontSize',24)
axis([0,nOutcomes+1,0,1])
legend('Status: Event occured','Status: Event did not occur')
set(gca,'xticklabel',nameThick)
set(gca,'FontSize',24)

cd(startpath)
end

author: mvallieres

repo: radiomics


text:
1     % problem
4     % grid parameter
1     % outflow boundary
2     % discretisation
0.01  % viscosity parameter
3     % Picard/Newton/hybrid linearization
2     % number of Picard iterations
4     % number of Newton iterations
1.d-5 % nonlinear tolerance
0.25  % Stokes stabilization parameter
1     % uniform/exponential streamlines

%% Data file for test problem NS1


text:
function [result] = caffe_anysize_test(function_handle, varargin)
% conv_anysize_test('init', param_file, model_file)
% conv_anysize_test('release')
% conv_anysize_test('set_input_size', height, width, channel, num)
% is_initialized = conv_anysize_test('is_initialized')
% response = conv_anysize_test('forward', img)

if strcmp(function_handle, 'forward')
    im = varargin{1};
    if size(im, 3) == 1
        im = im(:, :, [1, 1, 1]);
    end
    % convert to single
    im = single(im);
    % permute from RGB to BGR and subtract the data mean (already in BGR)
    data_mean = 128;
    im = im(:, :, [3 2 1]) - data_mean;
    % flip width and height to make width the fastest dimension, height
    % second
    im = permute(im, [2 1 3]);
    varargin{1} = im;

    [result] = caffe('forward', varargin);

    % result is in (width, height, channel), need trans to (height, width,
    % channel)
    for i = 1:length(result)
        result{i} = permute(result{i}, [2, 1, 3]);
    end
end

if strcmp(function_handle, 'set_input_size')
    varargin = cellfun(@(x) double(x), varargin, 'UniformOutput', false);
    caffe('set_input_size', varargin{:});
end

if strcmp(function_handle, 'init')
    caffe('init', varargin{:});
end

if strcmp(function_handle, 'is_initialized')
    result = caffe('is_initialized');
end

if strcmp(function_handle, 'release')
    caffe('release');
end

author: ShaoqingRen


text:
function [U_final, V_final, nIter_final, objhistory_final] = LCCF_Multi(K, k, W, options, U, V)
% Locally Consistant Concept Factorization (LCCF)
%
% where
%   X
% Notation:
% K ... (nSmp x nSmp) kernel matrix
% k ... number of hidden factors
% W ... weight matrix of the affinity graph
%
% options ... Structure holding all settings
%
% You only need to provide the above four inputs.
%
% X = X*U*V'
%
% References:
% [1] Deng Cai, Xiaofei He, Jiawei Han, "Locally Consistent Concept
%     Factorization for Document Clustering", IEEE Transactions on Knowledge
%     and Data Engineering, Vol. 23, No. 6, pp. 902-913, 2011.
%
%
%   version 2.0 --April/2010
%   version 1.0 --Dec./2008
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

differror = options.error;
maxIter = options.maxIter;
nRepeat = options.nRepeat;
minIterOrig = options.minIter;
minIter = minIterOrig-1;
meanFitRatio = options.meanFitRatio;

alpha = options.alpha;

Norm = 2;
NormV = 1;

nSmp = size(K,1);

if alpha < 0
    alpha = 0;
end
W = alpha*W;
DCol = full(sum(W,2));
D = spdiags(DCol,0,speye(size(W,1)));
L = D - W;
if isfield(options,'NormW') && options.NormW
    D_mhalf = DCol.^-.5;

    tmpD_mhalf = repmat(D_mhalf,1,nSmp);
    L = (tmpD_mhalf.*L).*tmpD_mhalf';
    clear D_mhalf tmpD_mhalf;

    L = max(L, L');
end

if isempty(U)
    U = abs(rand(nSmp,k));
    V = abs(rand(nSmp,k));
else
    nRepeat = 1;
end
[U,V] = NormalizeUV(K, U, V, NormV, Norm);

selectInit = 1;
if nRepeat == 1
    selectInit = 0;
    minIterOrig = 0;
    minIter = 0;
    if isempty(maxIter)
        [obj_NMFhistory, obj_Laphistory] = CalculateObj(K, U, V, L);
        objhistory = obj_NMFhistory + obj_Laphistory;
        meanFit = objhistory*10;
    else
        if isfield(options,'Converge') && options.Converge
            [obj_NMFhistory, obj_Laphistory] = CalculateObj(K, U, V, L);
            objhistory = obj_NMFhistory + obj_Laphistory;
        end
    end
else
    if isfield(options,'Converge') && options.Converge
        error('Not implemented!');
    end
end

tryNo = 0;
while tryNo < nRepeat
    tmp_T = cputime;
    tryNo = tryNo+1;
    nIter = 0;
    maxErr = 1;
    while(maxErr > differror)
        % ===================== update V ========================
        KU = K*U;               % n^2k
        UKU = U'*KU;            % nk^2
        VUKU = V*UKU;           % nk^2

        if alpha > 0
            WV = W*V;
            DV = repmat(DCol,1,k).*V;

            KU = KU + WV;
            VUKU = VUKU + DV;
        end

        V = V.*(KU./max(VUKU,1e-10));
        clear WV DV KU UKU VUKU;
        % ===================== update U ========================
        KV = K*V;               % n^2k
        VV = V'*V;              % nk^2
        KUVV = K*U*VV;          % n^2k

        U = U.*(KV./max(KUVV,1e-10));
        clear KV VV KUVV;

        nIter = nIter + 1;
        if nIter > minIter
            [U,V] = NormalizeUV(K, U, V, NormV, Norm);
            if selectInit
                [obj_NMFhistory, obj_Laphistory] = CalculateObj(K, U, V, L);
                objhistory = obj_NMFhistory + obj_Laphistory;
                maxErr = 0;
            else
                if isempty(maxIter)
                    [obj_NMF, obj_Lap] = CalculateObj(K, U, V, L);
                    newobj = obj_NMF + obj_Lap;
                    objhistory = [objhistory newobj]; %#ok<AGROW>
                    meanFit = meanFitRatio*meanFit + (1-meanFitRatio)*newobj;
                    maxErr = (meanFit-newobj)/meanFit;
                else
                    if isfield(options,'Converge') && options.Converge
                        [obj_NMF, obj_Lap] = CalculateObj(K, U, V, L);
                        newobj = obj_NMF + obj_Lap;
                        objhistory = [objhistory newobj]; %#ok<AGROW>
                    end
                    maxErr = 1;
                    if nIter >= maxIter
                        maxErr = 0;
                        if isfield(options,'Converge') && options.Converge
                        else
                            objhistory = 0;
                        end
                    end
                end
            end
        end
    end

    if tryNo == 1
        U_final = U;
        V_final = V;
        nIter_final = nIter;
        objhistory_final = objhistory;
    else
        if objhistory(end) < objhistory_final(end)
            U_final = U;
            V_final = V;
            nIter_final = nIter;
            objhistory_final = objhistory;
        end
    end

    if selectInit
        if tryNo < nRepeat
            %re-start
            U = abs(rand(nSmp,k));
            V = abs(rand(nSmp,k));

            [U,V] = NormalizeUV(K, U, V, NormV, Norm);
        else
            tryNo = tryNo - 1;
            minIter = 0;
            selectInit = 0;
            U = U_final;
            V = V_final;
            objhistory = objhistory_final;
            meanFit = objhistory*10;
        end
    end
end

nIter_final = nIter_final + minIterOrig;

Norm = 2;
NormV = 0;

[U_final,V_final] = NormalizeUV(K, U_final, V_final, NormV, Norm);

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

function [obj_NMF, obj_Lap, dV] = CalculateObj(K, U, V, L, deltaVU, dVordU)
if ~exist('deltaVU','var')
    deltaVU = 0;
end
if ~exist('dVordU','var')
    dVordU = 1;
end
dV = [];

UK = U'*K; % n^2k
UKU = UK*U; % nk^2
VUK = V*UK; % n^2k
VV = V'*V; % nk^2
obj_NMF = sum(diag(K))-2*sum(diag(VUK))+sum(sum(UKU.*VV));
if deltaVU
    if dVordU
        dV = V*UKU - UK'+ L*V; %nk^2
    else
        dV = (VUK-K)'*V;  %n^2k
    end
end
obj_Lap = sum(sum((L*V).*V));

function [U, V] = NormalizeUV(K, U, V, NormV, Norm)
k = size(U,2);
if Norm == 2
    if NormV
        norms = max(1e-15,sqrt(sum(V.^2,1)))';
        V = V*spdiags(norms.^-1,0,k,k);
        U = U*spdiags(norms,0,k,k);
    else
        norms = max(1e-15,sqrt(sum(U.*(K*U),1)))';
        U = U*spdiags(norms.^-1,0,k,k);
        V = V*spdiags(norms,0,k,k);
    end
else
    if NormV
        norms = max(1e-15,sum(abs(V),1))';
        V = V*spdiags(norms.^-1,0,k,k);
        U = U*spdiags(norms,0,k,k);
    else
        norms = max(1e-15,sum(U.*(K*U),1))';
        U = U*spdiags(norms.^-1,0,k,k);
        V = V*spdiags(norms,0,k,k);
    end
end

author: ZJULearning

repo: MatlabFunc


text:
function [U,Pm,Q1,Q2,R] = tps_set_landmarks(landmarks,ctrl_pts)
%%=====================================================================
%% $RCSfile: tps_set_landmarks.m,v $
%% $Author: bjian $
%% $Date: 2008/11/24 08:59:02 $
%% $Revision: 1.1 $
%%=====================================================================
[m,d] = size(landmarks);
U = compute_TPS_kernel(landmarks, ctrl_pts);
Pm = [ones(m,1) landmarks];
[q,r]   = qr(Pm);
Q1      = q(:, 1:d+1);
Q2      = q(:, d+2:m);
R       = r(1:d+1,1:d+1);

author: Sable

repo: mcbench-benchmarks


text:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% angular resolution
dt=pi/40;
t=0:dt:2*pi-dt;

%% parameters of a side-cut fiber
h=100;
r1=20;
r2=25;
a=-1;
b=0;
c=1;
d=-h;

%% key nodes of a side-cut fiber

n1=[r1*sin(t(:)) r1*cos(t(:)) zeros(size(t(:)))];
n2=[r2*sin(t(:)) r2*cos(t(:)) zeros(size(t(:)))];

n3=[r1*sin(t(:)) r1*cos(t(:)) -d-(a*r1*sin(t(:))+b*r1*cos(t(:)))/c];
n4=[r2*sin(t(:)) r2*cos(t(:)) -d-(a*r2*sin(t(:))+b*r2*cos(t(:)))/c];

no=[n1;n2;n3;n4];

%% PLCs of the side-cut fiber

clear fc;
count=1;
for i=1:length(t)-1
   % the last number in each cell is the fc id
   fc{count}={[i+length(t) i+3*length(t) i+3*length(t)+1 i+length(t)+1],1}; count=count+1;
   fc{count}={[i i+2*length(t) i+2*length(t)+1 i+1],2}; count=count+1;
end
i=length(t);
fc{count}={[i+length(t) i+3*length(t) 1+3*length(t) 1+length(t)],1}; count=count+1;
fc{count}={[i i+2*length(t) 1+2*length(t) 1],2}; count=count+1;

fc{count}={1:1+length(t)-1,3};count=count+1;  % bottom inner circle
fc{count}={[1+length(t):1+length(t)*2-1 nan fliplr(1:1+length(t)-1)],4};count=count+1; % button outter circle
fc{count}={1+length(t)*2:1+length(t)*3-1,5};count=count+1;  % top inner circle
fc{count}={[1+length(t)*3:1+length(t)*4-1 nan fliplr(1+length(t)*2:1+length(t)*3-1)],6}; % top outter circle

%% mesh generation of the cladding for the side-cut fiber
%[node,elem,face]=s2m(no,face,1,50);
[node,elem,face]=surf2mesh(no,fc,min(no),max(no),1,50,[0 0 1],[],0);

plotmesh(no,fc,'y>-0.1');
figure
plotmesh(node,elem,'x>0 | y>0');
figure;
plotmesh(node,face,'x>0 | y>0');

author: fangq


text:
function [RUMBA_outputs, UpRegulated, DownRegulated, MetConnectivity1, MetConnectivity2] = rumba(model1, model2, completeModel, sampling, maxMetConn, RxnsOfInterest, GenesOfInterest, NormalizePointsOption, PValCuttoff, MaxNumPoints, LoopRxnsToIgnore, verboseTag)
% RUMBA predicts which reactions significantly change their
% flux at metabolic branch points under two conditions
%
% USAGE:
%
%    [RUMBA_outputs, UpRegulated, DownRegulated, MetConnectivity1, MetConnectivity2] = rumba(model1, model2, completeModel)
% INPUTS:
%    model1:                     Model under first condition, exchange reactions
%                                are constrained with the data related to the first
%                                condition. If model already sampled
%                                ('sampling' = 0). The sampling points is in an
%                                mxn matrix with m reactions and n points included
%                                as a field in the model(i.e., `model1.points`).
%                                Set 'sampling' = 1 to set the model constrained
%                                under the first conditions
%    model2:                     Model under second condition. Same
%                                format as `model1`.
%    completeModel:              The complete reference model. This is used
%                                to verify consistency between the sampled models.
%
% OPTIONAL INPUTS:
%    sampling:                   0, if no sampling needed (default)
%                                1, if sampling of the models under both
%                                conditions
%    maxMetConn:                 The maximum connectivity of a metabolite to
%                                consider. All branch points with a higher
%                                connectivity will be ignored (default = 30)
%    RxnsOfInterest:             Reactions for which predictions are desired.
%                                Specifying only desired reactions speeds up
%                                algorithm (default = all reactions)
%    GenesOfInterest:            Genes associated with the reactions of interest (default = all genes)
%    NormalizePointsOption:      Option to normalize sample points to (1) the
%                                same median of magnitude of flux through all
%                                non-loop gene-associated reactions, or (2) the
%                                optimal growth rate. (default = 1)
%    PValCuttoff:                P-value cutoff used to decide which changes in
%                                branch point flux to call significant (two-
%                                tailed p-value, so .05 will mean 0.25 on both
%                                tails). (default = 0.05)
%    MaxNumPoints:               Maximum number of points to use form the
%                                sampled models. Extra points will be removed
%                                to improve memory usage and speed up
%                                calculations. (default = minimum number of
%                                points in the model or 500 points, whichever
%                                is smaller)
%    verboseTag:                 1 = print out progress and use waitbars (default).
%			 	 0 = print only minimal progress to screen.
%    LoopRxnsToIgnore:           list of rxns associated with loop within the model,
%                                (default - reaction loops defined using FVA)
%
% OUTPUTS:
%    RUMBA_outputs:              Structure containing all information about
%                                each gene-reaction pair. For gene-reaction pair
%                                for which the differential branch-point
%                                information is possible to calculate: the list of
%                                connected metabolites ('ConnectedMets'), the associated
%                                up-regulation p-value ('pValue_up'), the associated
%                                down-regulation p-value ('pValue_down')and
%                                the reaction directionality for both model
%                                ('direction').The structure also contains the list of gene-reaction
%                                pairs for which no differential branch-point
%                                information can be determined (because of
%                                loops, unused pathways, etc.).
%    UpRegulated:                first column : Gene-reaction pairs towards which flux is
%                                significantly upregulated during the shift;
%                                second column : list of metabolites
%                                connected to the gene-reaction pair;
%                                third column : Magnitude of absolutes flux change
%    DownRegulated:              same structure as `UpRegulated` but for gene-reaction pairs
%                                from which flux is significantly
%                                downregulated during the shift
%    MetConnectivity1:           A structure that present for each metabolite present in model1
%                                the following sets of fields:
%
%                                  * ConnRxns - the reactions that are connected
%                                    to the metabolite
%                                  * Sij - The stoichiometric coefficient for the
%                                    metabolite in each reaction in ConnRxns
%                                  * RxnScore - Score for each reaction in ConnRxns
%                                  * Direction - The direction of reaction flux
%                                    for each sample point
%                                  * MetNotUsed - Whether or not the metabolite is
%                                    used in the condition
%    MetConnectivity2:           Same as `MetConnectivity1` but for `model2`
%
%
% .. Authors:
%       - Nathan E. Lewis, May 2010-May 2011
%       - Anne Richelle, May 2017

if nargin < 12  || isempty(verboseTag) % Set the function to not use waitbars and lots of status text.
    verboseTag = 0;
end
if nargin < 11  || isempty(LoopRxnsToIgnore)
    tmp = completeModel;
    tmpExc = findExcRxns(tmp);
    tmp.lb(tmpExc) = 0;
    tmp.lb(tmp.lb >0) = 0;
    tmp.ub(tmpExc) = 0;
    [MinFVA MaxFVA] = fluxVariability(tmp,0,'max',tmp.rxns);
    LoopRxnsToIgnore = tmp.rxns(or(MinFVA<-1e-10, MaxFVA>1e-10));
    LoopRxnsToIgnore = {};
    tmpRxnForm = printRxnFormula(tmp,LoopRxnsToIgnore);
end

% Set the minimum p-value cutoff to 0.05 for determining significant shifts at metabolites
if nargin <9 || isempty(PValCuttoff)
    PValCuttoff = 0.05;
end

% Default is to normalize the points between the two models by net flux
if nargin < 8  || isempty(NormalizePointsOption)
    NormalizePointsOption = 1;
end

% If no set of genes is provided, look at all genes
if nargin <6
    [tmp_r,tmp_r2] = findRxnsFromGenes(completeModel, completeModel.genes, 0, 1);
    RxnsOfInterest = tmp_r2(:,1);
    GenesOfInterest = tmp_r2(:,5);
end

% if no metabolite connectivity filter is provided, filter out all
% metabolite with a connectivity greater than 30
if nargin < 5 || isempty(maxMetConn)
    maxMetConn = 30;
end

% if no sampling option, models are already sampled
if nargin < 4 || isempty(sampling)
    sampling = 0;
    if ~isfield(model1,'points'),
        warning 'Model1 is not sampled, sampling will be performed using sampleCbModel'
        sampling = 1;
    end
    if ~isfield(model2,'points'),
        warning 'Model2 is not sampled, sampling will be performed using sampleCbModel'
        sampling = 1;
    end
end

if sampling == 1
    %Sampling of the model 1
    [model1Sampling,samples1] = sampleCbModel(model1, 'model1Sampling');
    model1=model1Sampling;
    model1.points=samples1;
    %Sampling of the model 2
    [model2Sampling,samples2] = sampleCbModel(model2, 'model2Sampling');
    model2=model2Sampling;
    model2.points=samples2;
end

% Set the maximum number of points to look at to 500 or the minimum number of points in the model
if nargin < 10 || isempty(MaxNumPoints)
    MaxNumPoints = min([500;length(model1.points(1,:)); length(model2.points(1,:))]);
end

% since the statistics should be two-tailed, divide the p-value by 2
PValCuttoff = PValCuttoff/2;

% preprocessing step: Make sure all reactions from complete model are
% are in the sampled models
model1 = addMissingReactions(model1, completeModel);
model2 = addMissingReactions(model2, completeModel);

% Normalize the sampled points by scaling by the net network flux (1 = by
% net flux, 2 = by growth rate
    [model1,model2] = normalizePoints(model1, model2, NormalizePointsOption, LoopRxnsToIgnore);

% Rename genes to avoid incompatibilities
model1.genes = regexprep(model1.genes,'[_\-]','');% remove underscores and hyphens
model2.genes = regexprep(model2.genes,'[_\-]','');
completeModel.genes = regexprep(completeModel.genes,'[_\-]','');
GenesOfInterest = regexprep(GenesOfInterest,'[_\-]','');

% check to make sure there is enough sample points
if MaxNumPoints > min([length(model1.points(1,:)); length(model2.points(1,:))])
    MaxNumPoints = min([length(model1.points(1,:)); length(model2.points(1,:))]);
    warning('Number of sample points desired is more than available number in models!')
    display(cat(2,'you need to use ',num2str(MaxNumPoints),' points.'))
end

% for each metabolite get all incoming and outgoing reaction and score them
display('Processing first condition.')
[MetConnectivity1, ConnectedMet1] = classifyRxns(completeModel, model1, maxMetConn, MaxNumPoints, verboseTag, LoopRxnsToIgnore);
display('Processing second condition.')
[MetConnectivity2, ConnectedMet2] = classifyRxns(completeModel, model2, maxMetConn, MaxNumPoints, verboseTag, LoopRxnsToIgnore);
display('Comparing conditions.')
[MetsAndRxns,pVal_up,pVal_down,Dir_model1,Dir_model2] = compareConditions(MetConnectivity1, ConnectedMet1, MetConnectivity2, ConnectedMet2);

% process the 'compareCondtions' results
RUMBA_Struc_Pred = struct;
RUMBA_Struc_noPred = struct;
UpRegulated = {};
DownRegulated = {};
UpRegulated_mets = {};
DownRegulated_mets = {};
RUMBA_outputs=struct;
noPred={}

display('Classifying reaction/gene pair changes')
% step through all reactions of interest and determine if they
% significantly change
for i=1:length(RxnsOfInterest)
    % find whether split flux is increasing or decreasing for each reaction
    Ind = find(ismember(MetsAndRxns(:,2),RxnsOfInterest{i}));
    if ~isempty(Ind)

        Pred.ConnectedMets = regexprep(MetsAndRxns(Ind,1),'Met_',''); % metabolites to which the reaction is connected
        Pred.pValue_up = pVal_up(Ind); % p-value that flux is diverted to this reaction in each node in which it participates
        Pred.pValue_down = pVal_down(Ind); % p-value that flux is diverted away from this reaction in each node in which it participates
        Pred.Direction = [Dir_model1(Ind) Dir_model2(Ind)]'; % reaction directionality for both model (1 producing metabolite, -1 consuming the metabolite)

        RUMBA_outputs.(cat(2,GenesOfInterest{i},'_',RxnsOfInterest{i}))=Pred;

        % if the rxn/gene pair goes up for at least one metabolite, but not
        % down, and is above the p-value cutoff, then put it in the list of
        % gene-reaction pairs that significantly go up
        if min(pVal_up(Ind))<PValCuttoff && max(pVal_up(Ind))<=(1-PValCuttoff)
            UpRegulated{end+1} = cat(2,GenesOfInterest{i},'_',RxnsOfInterest{i});
            UpRegulated_mets{end+1} = regexprep(MetsAndRxns(Ind(pVal_up(Ind)<PValCuttoff),1),'Metab_','');
        end

        % if the rxn/gene pair goes down for at least one metabolite, but
        % not up  and is above the p-value cutoff, then put it in the list
        % of gene-reaction pairs that significantly go down
        if min(pVal_down(Ind))<PValCuttoff && max(pVal_down(Ind))<=(1-PValCuttoff)
            DownRegulated{end+1} = cat(2,GenesOfInterest{i},'_',RxnsOfInterest{i});
            DownRegulated_mets{end+1} = regexprep(MetsAndRxns(Ind(pVal_down(Ind)<PValCuttoff),1),'Metab_','');
        end

    else
        noPred{end+1}=([GenesOfInterest{i},'_',RxnsOfInterest{i}]);
    end
end
RUMBA_outputs.NotPredicted=noPred;

[UpRegulated,Ind] = sort(UpRegulated');
UpRegulated_mets = UpRegulated_mets(Ind)';
[DownRegulated,Ind] = sort(DownRegulated');
DownRegulated_mets = DownRegulated_mets(Ind)';

%filter out genes that go up and down
tmpUp = regexprep(UpRegulated,'_[0-9A-Za-z\-_\.\,\''\"\(\)\[\]]+$','');    % get all up gene IDs
tmpDwn = regexprep(DownRegulated,'_[0-9A-Za-z\-_\.\,\''\"\(\)\[\]]+$',''); % get all down gene IDs
ToDelU = ismember(tmpUp,tmpDwn);                                           % find Up genes also in the Down category
ToDelD = ismember(tmpDwn,tmpUp);                                           % find Down genes also in the Up category

UpRegulated(ToDelU)=[];
DownRegulated(ToDelD)=[];
UpRegulated_mets(ToDelU)=[];
DownRegulated_mets(ToDelD)=[];

UpRegulated = ([UpRegulated UpRegulated_mets]);
DownRegulated = ([DownRegulated DownRegulated_mets]);

% rank each reaction by the median magnitude change
[rxnsInCommon,MedRxnChange] = pValDistForModelOverlap(model1,model2);
tmpUp = regexprep(UpRegulated(:,1),'^[0-9A-Za-z\-\.\,\''\"\(\)\[\]]+_','');
tmpDwn = regexprep(DownRegulated(:,1),'^[0-9A-Za-z\-\.\,\''\"\(\)\[\]]+_','');

MagnitudeUp = zeros(length(tmpUp),1);
for i = 1:length(tmpUp)
	Ind = find(ismember(rxnsInCommon,tmpUp{i}));
	if isempty(Ind)
    	display(cat(2,'Reaction: ',tmpUp{i},' is missing!'))
    elseif length(Ind)>1
    	display(cat(2,'Reaction: ',tmpUp{i},' is duplicated in model!'))
    else
    	MagnitudeUp(i) = abs(MedRxnChange(Ind));
    end
end

MagnitudeDown = zeros(length(tmpDwn),1);
for i = 1:length(tmpDwn)
	Ind = find(ismember(rxnsInCommon,tmpDwn{i}));
	if isempty(Ind)
    	display(cat(2,'Reaction: ',tmpDwn{i},' is missing!'))
    elseif length(Ind)>1
        display(cat(2,'Reaction: ',tmpDwn{i},' is duplicated in model!'))
    else
    	MagnitudeDown(i) = abs(MedRxnChange(Ind));
    end
end

UpRegulated = ([UpRegulated num2cell(MagnitudeUp)]);
DownRegulated = ([DownRegulated num2cell(MagnitudeDown)]);

end


text:
% Minimize stopband ripple of a linear phase lowpass FIR filter
% "Filter design" lecture notes (EE364) by S. Boyd
% (figures are generated)
%
% Designs a linear phase FIR lowpass filter such that it:
% - minimizes the maximum passband ripple
% - has a constraint on the maximum stopband attenuation
%
% This is a convex problem.
%
%   minimize   delta
%       s.t.   1/delta <= H(w) <= delta     for w in the passband
%              |H(w)| <= atten_level        for w in the stopband
%
% where H is the frequency response function and variables are
% delta and h (the filter impulse response).
%
% Written for CVX by Almir Mutapcic 02/02/06

%********************************************************************
% user's filter specifications
%********************************************************************
% filter order is 2n+1 (symmetric around the half-point)
n = 10;

wpass = 0.12*pi;        % passband cutoff freq (in radians)
wstop = 0.24*pi;        % stopband start freq (in radians)
atten_level = -30;      % stopband attenuation level in dB

%********************************************************************
% create optimization parameters
%********************************************************************
N = 30*n+1;                            % freq samples (rule-of-thumb)
w = linspace(0,pi,N);
A = [ones(N,1) 2*cos(kron(w',[1:n]))]; % matrix of cosines

% passband 0 <= w <= w_pass
ind = find((0 <= w) & (w <= wpass));   % passband
Ap  = A(ind,:);

% transition band is not constrained (w_pass <= w <= w_stop)

% stopband (w_stop <= w)
ind = find((wstop <= w) & (w <= pi));  % stopband
Us  = 10^(atten_level/20)*ones(length(ind),1);
As  = A(ind,:);

%********************************************************************
% optimization
%********************************************************************
% formulate and solve the linear-phase lowpass filter design
cvx_begin
  variable delta
  variable h(n+1,1);

  minimize( delta )
  subject to
    % passband bounds
    Ap*h <= delta;
    inv_pos(Ap*h) <= delta;

    % stopband bounds
    abs( As*h ) <= Us;
cvx_end

% check if problem was successfully solved
disp(['Problem is ' cvx_status])
if ~strfind(cvx_status,'Solved')
  return
else
  % construct the full impulse response
  h = [flipud(h(2:end)); h];
  fprintf(1,'The optimal minimum passband ripple is %4.3f dB.\n\n',...
            20*log10(delta));
end

%********************************************************************
% plots
%********************************************************************
figure(1)
% FIR impulse response
plot([0:2*n],h','o',[0:2*n],h','b:')
xlabel('t'), ylabel('h(t)')

figure(2)
% frequency response
H = exp(-j*kron(w',[0:2*n]))*h;
% magnitude
subplot(2,1,1)
plot(w,20*log10(abs(H)),[wstop pi],[atten_level atten_level],'r--');
axis([0,pi,-40,10])
xlabel('w'), ylabel('mag H(w) in dB')
% phase
subplot(2,1,2)
plot(w,angle(H))
axis([0,pi,-pi,pi])
xlabel('w'), ylabel('phase H(w)')

author: cvxr

repo: CVX

save_path: github-repos/MATLAB/cvxr-CVX


text:
function out = double(fa)
% Convert to double precision
% FORMAT double(fa)
% fa - a file_array
% _______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

%
% $Id$

out = double(numeric(fa));


text:
function [poly,poly_idx,max_index,max_size] = extdom_polygon(bnde,pts,order,line,min_size)
% DESCRIPTION: Given a set of boundary edges of a singly- or multi-
%              polygonal region, organize them in a winding order.
%
% INPUTS:
%         bnde: the indices of each boundary edge as a nbnde x 2 matrix
%         pts:  the x,y locations of all the points in the region
%               stored as an np x 2 matrix.
%         order:the order in which the traversal takes place
%               counter-clockwise (0) or clockwise (1) or add a negative
%               sign to append NaNs in each cell.
%          line:if desired output will be polylines
%       min_size: if the lenght of the polygon is less than min_size
%                  points, throw it out. zero by default.
% OUTPUTS:
%          poly: the boundary of each enclosing polygon sorted in winding-order
%                poly is returned as a cell-array of length number of polys.
%      poly_idx: indices of the polygon coordinates in the same format as
%                poly
%     max_index: is the index into poly that is the largest
%      max_size: is the size of the largest poly
%
% Last Edited:
% kjr,UND,CHL,2017
%   kjr,UND,CHL -->revised for massive speed improvements March 2018.
%
%                                           TRAVERSAL METHOD
% Pick any unvisited edge segment [v_start,v_next] and add these vertices to the polygon loop.
% Find the unvisited edge segment [v_i,v_j] that has either v_i = v_next or v_j = v_next and add the other vertex (the one not equal to v_next) to the polygon loop.
% Reset v_next as this newly added vertex, mark the edge as visited and continue from 2.
% Traversal is done when we get back to v_start.
% NOTE: that the signed area will be positive if the vertices are
% oriented counterclockwise, and will be negative if it is oriented clockwise

% NOTE: By flipping the edges left-to-right, we can easily see the nodal
% connectivity of the triangulation. However, since we are effectively
% adding new "edges", we must also quickly locate and flag the "flipped" edge
% in addition to the current edge under consideration. This is accomplished with the invmap
% array which allows one, given a vertex gid, to quickly find it's linear
% index inside the array bnde((boundary edges). This localizes the search to find
% the connectivity to continue the "walk" on the boundary making the
% calculation massively more efficient than compared to searching the entire bnde
% array with edge under consideration.

% if storing lines
if(nargin < 4); line = 0; end
if(nargin < 5); min_size = 1; end

bnde = [bnde; fliplr(bnde)];
bnde = sortrows(bnde,1);
ned = length(bnde);
an  = sign(order);
active = true(ned,1);

p = 0 ;
% given a vertex with num gid, return where it exists last lid.
for lid = 1 : ned
    gid = bnde(lid,1);
    invmap(gid) = lid ;
end

while any(active)
    p = p + 1;

    rn = find(active,1);

    temp  = pts(bnde(rn,:)',:);
    temp2 = bnde(rn,:)';

    v_start= bnde(rn,1);
    v_next = bnde(rn,2);

    active(rn) = false;

    % flag flipped edge too
    flipped = fliplr(bnde(rn,:));
    idx = invmap(flipped(1));
    st   = max((idx - 1),1);
    ed   = min(idx,ned);
    rn = find(flipped(2)==bnde(st:ed,2),1);

    active(rn+st-1) = false;

    k = 2 ;
    while v_next~=v_start

        % form local set to search for continuation of boundary walk.
        idx  = invmap(v_next);
        st   = max((idx - 1),1);
        ed   = min(idx,ned);

        r = find(v_next==bnde(st:ed,1) & active(st:ed),1);
        tsel = bnde(st+r-1,:); tsel_inv = fliplr(tsel) ;
        sel=tsel(tsel~=v_next);

        if(line)
            if(isempty(sel))
                break
            end
        end
        % store points.
        k = k + 1;
        temp(k,:) = pts(sel,:);
        temp2(k,:)= sel;

        active(r+st-1) = false;

        % flag flipped edge too
        idx = invmap(tsel_inv(1));
        st   = max((idx - 1),1);
        ed   = min(idx,ned);
        r = find(tsel_inv(2)==bnde(st:ed,2),1);
        active(r+st-1) = false;

        v_next = sel;

    end

    if length(temp > min_size)
        poly{p}     = temp;
        poly_idx{p} = temp2;
    else
        continue
    end
    if an < 0
        poly{p}(end+1,:) = [NaN NaN];
        poly_idx{p}(end+1,:) = NaN;
    end
    [area]=parea(poly{p}(:,1),poly{p}(:,2));
    if order==0 % ccw
        if sign(area)<0
            poly{p} = flipud(poly{p});
            poly_idx{p} = flipud(poly_idx{p});
        end
    else % cw
        if sign(area)>0
            poly{p} = flipud(poly{p});
            poly_idx{p} = flipud(poly_idx{p});
        end
    end
end
[max_size, max_index] = max(cellfun('size', poly, 1));
end
% helper function, computes area of polygon
function [area]=parea(x,y)
n    = length(x);
xp   = [x; x(1)];
yp   = [y; y(1)];
area = 0;
for i = 1:n
    area = area + det([xp(i), xp(i+1); yp(i), yp(i+1)]);
end
area = 1/2*area;
end

author: CHLNDDEV