function p=v_psychofunc(m,q,x,r)
%V_PSYCHOFUNC Calculate psychometric functions: trial success probability versus SNR
%
% Usage: p=v_psychofunc('',q,x)       % calculate probabilities
%        b=v_psychofunc('r',q,x)       % generate boolean variables with success prob p
%        p=v_psychofunc(m,q,x,r)     % Calculate likelihoods for observations r
%        x=v_psychofunc([m 'i'],q,p) % Calculate inverse
%
% Inputs:
%          m        mode string [may be omitted if not required]
%                      'n'   do not normalize likelihoods
%                      'f'   do not squeeze output arrays to remove singleton dimensions
%                      'i'   calculate inverse function
%                      'r'   calculate binary random variables with probability p
%                      ['s'   calculate sweet points for threshold and slope]
%                      ['d'   calculate partial derivatives with respect to q(1:5)]
%                      'g'   plot graph
%                      'G'   plot image
%                      'c'   include colourbar
%          q        model parameters. Either a column vector with a single model,
%                   a matrix with one model per column or a cell array with multiple values for
%                   some or all of the parameters
%                      1  probability at threshold [0.5]
%                      2  threshhold [0 dB]
%                      3  slope at threshold [0.1 prob/dB ]
%                      4  miss or lapse probability [0]
%                      5  guess probability   [0]
%                      6  psychometric function type [1]
%                          1 = logistic
%                          2 = cumulative Gaussian
%                          3 = Weibull
%                          [4 = reversed Weibull]
%                          [5 = Gumbell]
%                          [6 = reversed Gumbell]
%          x        vector of SNR values
%          r        test results (0 or 1) corresponding to x
%          p        vector of probabilities
%
% Outputs:
%          p        array of probabilities or random variates ('r' option).
%                   p is a squeezed 7-dimensional array
%                   whose dimensions correspond to x followed by the 6 model parameter entries.
%                   if q is a cell array, singleton dimensions are removed unless the 'f' option is given.
%          x        Inverse function gives SNR, x, as a function of p
%          b        array of boolean variables

%      Copyright (C) Mike Brookes 2009-2010
%      Version: $Id: v_psychofunc.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% first sort out input arguments
minp=0.01;          % minimum probability to use for inverse function by default
qq=[0.5 0 0.1 0 0 1]';  % default values for q
if nargin<4
    r=[];
    if nargin<3
        x=[];
        if nargin<2
            q=[];
            if ~nargin
                m='';
            end
        end
    end
end
if ~ischar(m);      % mode argument is optional
    r=x;
    x=q;
    q=m;
    m='';
end
sq=size(q);
ckmod=0;
if iscell(q)
    nq=ones(1,6);
    qax=num2cell([0; qq]);  % used for plotting
    for i=1:min(numel(q),6)
        nq(i)=numel(q{i});
        if nq(i)>=1
            nr=size(qq,2);
            qax{i+1}=q{i};
            if i<=5             % do not replicate for multiple models
                qq=repmat(qq,1,nq(i));
                qq(i,:)=reshape(repmat(q{i}(:)',nr,1),1,nr*nq(i));
            else
                qq(i,:)=repmat(q{i}(1),1,nr);
            end
        end
    end
    nq=max(nq,1);
    nmod=nq(6);
    if nmod>1      % list of models to use
        modlist=q{6};
    else
        modlist=qq(6,1);  % default model
    end
else
    nq=sq(2);
    if nq
        ql=repmat(qq,1,nq);
        ql(1:sq(1),:)=q;
    else
        ql=qq;
        nq=1;
    end
    modlist=unique(ql(6,:));
    nmod=length(modlist);
    ckmod=nmod>1;   % need to check model list
    qq=ql;
end
% now perform the calculation
nx=numel(x);
npt=50;       % number of points
if any(m=='i') % doing inverse
    if ~nx
        nx=npt;
        xlim=[max(qq(5,:)),1-max(qq(4,:))]*[1-minp minp; minp 1-minp];
        x=linspace(xlim(1),xlim(2),nx)';
    end
    p=zeros([nx nq]);  % space for SNRs
    ia=0;
    for i=1:nmod % loop for each model type
        mod=modlist(i);
        if ckmod
            qq=ql(:,ql(6,:)==mod);
        end
        pscale=1-qq(4,:)-qq(5,:);
        pstd=(qq(1,:)-qq(5,:))./pscale; % prob target compensating for miss and lapse probs
        sstd=qq(3,:)./pscale; % slope compensating for miss and lapse probs
        px=x(:)*pscale.^(-1)-repmat(qq(5,:)./pscale,nx,1);  % adjust for miss and lapse probs
        switch mod
            case 1
                beta=sstd./(pstd.*(1-pstd));
%                 alpha=qq(2,:)+log((1-pstd)./pstd)./beta;
                px=repmat(qq(2,:)+log((1-pstd)./pstd)./beta,nx,1)-log(px.^(-1)-1).*repmat(beta.^(-1),nx,1);
            case 2   % cumulative Gaussian function
                xtstd=norminv(pstd); % x position of target in std measure
                sig=normpdf(xtstd)./sstd;
                px= repmat(qq(2,:)-sig.*xtstd,nx,1) + repmat(sig,nx,1).*norminv(px);
            case 3
                wlog=log(1-pstd);
                kbeta=sstd./((pstd-1).*wlog);
                alpha=qq(2,:)-log(-wlog)./kbeta;
                px=repmat(alpha,nx,1)+log(-log(1-px)).*repmat(kbeta.^(-1),nx,1);
            otherwise
                error('Invalid psychometric model index');
        end
        if ckmod
            p(:,ql(6,:)==i)=px;
        else
            ib=ia+numel(p)/nmod;
            p(ia+1:ib)=px(:);
            ia=ib;
        end
    end
else % doing forward mapping
    if ~nx
        ef=2;         % expansion factor
        nx=npt;
        x=linspace(min(qq(2,:)-ef*(qq(1,:)-qq(5,:))./qq(3,:)), ...
            max(qq(2,:)+ef*(1-qq(1,:)-qq(4,:))./qq(3,:)),nx)';
    end
    p=zeros([nx nq]);  % space for probabilities
    ia=0;
    for i=1:nmod % loop for each model type
        mod=modlist(i);
        if ckmod
            qq=ql(:,ql(6,:)==mod);
        end
        pscale=1-qq(4,:)-qq(5,:);  % prob range excluding miss and lapse probs
        pstd=(qq(1,:)-qq(5,:))./pscale; % prob target compensating for miss and lapse probs
        sstd=qq(3,:)./pscale; % slope compensating for miss and lapse probs
        switch mod
            case 1   % logistic function
                beta=sstd./(pstd.*(1-pstd));
%                 alpha=qq(2,:)+log((1-pstd)./pstd)./beta;
                px=(1+exp(repmat(beta.*qq(2,:)+log((1-pstd)./pstd),nx,1)-x(:)*beta)).^(-1);
            case 2   % cumulative Gaussian function
                xtstd=norminv(pstd); % x position of target in std measure
                sigi=sstd./normpdf(xtstd);
                px=normcdf(x(:)*sigi-repmat(qq(2,:).*sigi-xtstd,nx,1));
            case 3
                wlog=log(1-pstd);
                kbeta=sstd./((pstd-1).*wlog);
                alpha=qq(2,:)-log(-wlog)./kbeta;
                px=1-exp(-exp(x(:)*kbeta-repmat(alpha.*kbeta,nx,1)));
            otherwise
                error('Invalid psychometric model index');
        end
        px=repmat(qq(5,:),nx,1)+repmat(pscale,nx,1).*px;  % adjust for miss and lapse probs
        if ckmod
            p(:,ql(6,:)==i)=px;
        else
            ib=ia+numel(p)/nmod;
            p(ia+1:ib)=px(:);
            ia=ib;
        end
    end
    if numel(r)                 % we are calculating likelihoods
        mk=r(:)==0;
        p(mk,:)=1-p(mk,:);      % invert probability for results that are zero
        if nx>1
            if any(m=='n')
                p=prod(p,1);
            else
                p=sum(log(p),1);
                p=exp(p-max(p(:)));
                p=p/sum(p(:));     % normalize to equal 1
            end
            nx=1;
        end
    end

end
pg=p;       % save unsqueezed p for plotting
if ~any(m=='f') && iscell(q) % remove all singleton dimensions
    szp=size(p);
    szq=szp(szp>1);
    szq=[szq ones(1,max(0,2-numel(szq)))];
    p=reshape(p,szq);
end
if any(m=='r') && ~any(m=='i');
    p=rand(size(p))<p;
end

if ~nargout || any(lower(m)=='g')
    clf;
    szp=[nx nq];
    czp=sum(szp>1);
    if czp>0  % check if there is anything to plot
        if iscell(q)
            axlab={'Input SNR','Threshold prob','Threshold SNR','Threshold slope','Lapse prob','Guess prob','Sigmoid type'};
            [szs,izs]=sort(szp,'descend');
            pg=permute(pg,izs);
            qax{1}=x;
            if any(m=='G') || czp>2 % image
                ngr=prod(szs(3:end));
                ncol=ceil(sqrt(ngr));
                nrow=ceil(ngr/ncol);
                npix=szs(1)*szs(2);
                ia=0;
                for i=1:ngr
                    subplot(nrow,ncol,i);
                    ib=ia+npix;
                    imagesc(qax{izs(1)},qax{izs(2)},reshape(pg(ia+1:ib),szs(1:2))');
                    axis 'xy'
                    if any(m=='c')
                        colorbar;
                    end
                    if nrow*ncol-i<ncol
                        xlabel(axlab(izs(1)));
                    end
                    if rem(i-1,ncol)==0
                        ylabel(axlab(izs(2)));
                    end
                    ia=ib;
                end
            else                    % graph
                plot(qax{izs(1)},reshape(permute(pg,izs),szs(1:2)),'-');
                xlabel(axlab{izs(1)});
            end
        else
            if any(m=='G')  % image
                imagesc(pg');
                axis 'xy'
                if any(m=='c')
                    colorbar;
                end
                xlabel('Input SNR (dB)');
                ylabel('Model Index');
            else            % graph
                if nx>=nq
                    plot(x,pg,'-');
                    xlabel('Input SNR (dB)');
                else
                    plot(1:nq,pg','-');
                    xlabel('Model Index');
                end
            end
        end
    end
end


% TRANSFORMCOORDS - Select nazion and inion in anatomical MRI images.
%
% Usage:
%   mewcoords = transformcoords(coords, rotate, scale, center, reverse);
%
% Inputs:
%   coords    - array of 3-D coordinates (3 by N or N by 3)
%   rotate    - [pitch roll yaw] rotate in 3-D using pitch (x plane),
%               roll (y plane) and yaw (z plane). An empty array does
%               not perform any rotation.
%   scale     - [scalex scaley scalez] scale axis. A single numeric
%               input scale all the dimensions the same. Default 1
%               does not scale.
%   shifts    - [x y z] shift coordinates (after rotation and scaling).
%               Default [0 0 0] does not move the center.
%   reverse   - [0|1] when set to 1 perform the reverse transformation,
%               first moving to the old center, unscaling, and unrotating.
%               Default is 0.
%
% Output:
%   newcoords - coordinates after rotating, scaling and recentering
%
% Author: Arnaud Delorme, Salk, SCCN, UCSD, CA, March 23, 2004

% Copyright (C) 2004 Arnaud Delorme
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function coords = transformcoords(coords, rotate, scale, center, reverse);

    if nargin < 2
        help transformcoords;
        return;
    end
    if nargin < 3
        scale =1;
    end
    if nargin < 4
        center = [0 0 0];
    end
    if nargin < 5
        reverse = 0;
    end
    if size(coords, 1) ~= 3
        coords = coords';
        trp    = 1;
    else
        trp = 0;
    end
    if size(coords, 1) ~= 3
        error('Number of columns must be 3 for the coordinate input');
    end
    if length(rotate) > 0 && length(rotate) ~= 3
        error('rotate parameter must have 3 values');
    end

    % decode parameters
    % -----------------
    centx = -center(1);
    centy = -center(2);
    centz = -center(3);
    if length(scale) == 1
        scale = [scale scale scale];
    end
    scalex = scale(1);
    scaley = scale(2);
    scalez = scale(3);
    if length(rotate) < 3
        rotate = [0 0 0]
    end
    pitch = rotate(1);
    roll  = rotate(2);
    yaw   = rotate(3);

    if ~reverse
        % pitch roll yaw rotation
        % -----------------------
        % pitch (x-axis); roll = y axis rotation; yaw = z axis
        % see http://bishopw.loni.ucla.edu/AIR5/homogenous.html
        cp = cos(pitch); sp = sin(pitch);
        cr = cos(roll);  sr = sin(roll);
        cy = cos(yaw);   sy = sin(yaw);
        rot3d = [ cy*cr+sy*sp*sr    sy*cr-cy*sp*sr     cp*sr  ;
                  -sy*cp            cy*cp               sp     ;
                  sy*sp*cr-cy*sr    -cy*sp*cr-sy*sr     cp*cr  ];
        coords = rot3d*coords;

        % scaling and centering
        % ---------------------
        coords(1,:) = coords(1,:)*scalex-centx;
        coords(2,:) = coords(2,:)*scaley-centy;
        coords(3,:) = coords(3,:)*scalez-centz;
    else
        % unscaling and uncentering
        % -------------------------
        coords(1,:) = (coords(1,:)+centx)/scalex;
        coords(2,:) = (coords(2,:)+centy)/scaley;
        coords(3,:) = (coords(3,:)+centz)/scalez;

        % pitch roll yaw rotation
        % -----------------------
        cp = cos(-pitch); sp = sin(-pitch);
        cr = cos(-roll);  sr = sin(-roll);
        cy = cos(-yaw);   sy = sin(-yaw);
        rot3d = [ cy*cr+sy*sp*sr    sy*cr-cy*sp*sr     cp*sr  ;
                  -sy*cp            cy*cp               sp     ;
                  sy*sp*cr-cy*sr    -cy*sp*cr-sy*sr     cp*cr  ];
        coords = rot3d*coords;
    end

    if trp
        coords = coords';
    end


% MP_SOLVE_STOKES_DIV_CONFORMING: Solve a Stokes flow problem on a multipatch domain with
% divergence-conforming spaces, using an interior penalty method between patches.
%
% The function solves the Stokes problem
%
%   -div(mu(x) grad(vel)) + grad(press) = f    in Omega
%                              div(vel) = 0    in Omega
%                                   vel = h    on Gamma_D
%
% USAGE:
%
%  [geometry, msh, space_v, vel, space_p, press] = ...
%                        mp_solve_stokes_div_conforming (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - f:            force term
%    - h:            function for Dirichlet boundary condition
%    - viscosity:    viscosity coefficient (mu in the equation)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:       degree of the spline functions for pressure
%    - regularity:   continuity of the spline functions for pressure
%    - nsub:       number of subelements with respect to the geometry mesh
%                   for the pressure space (nsub=1 leaves the mesh unchanged)
%    - nquad:        number of points for Gaussian quadrature rule
%    - element_name: one of {RT,NDL}, specify how to build the velocity
%                    space from the data for the pressure space
%                     +RT  is the generalized Raviart-Thomas element
%                     +NDL is the generalized Nedelec element of the second family
% OUTPUT:
%
%  geometry: array of geometry structures (see geo_load)
%  msh:      multipatch mesh, consisting of several Cartesian meshes (see msh_multipatch)
%  space_v:  multipatch space, formed by several tensor product spaces plus the connectivity (see sp_multipatch).
%              Only the normal component is continuous at the interfaces.
%  vel:      the computed degrees of freedom for the velocity
%  space_p:  multipatch space for the pressure (see sp_multipatch). The functions are discountinuous at the interfaces.
%  press:    the computed degrees of freedom for the pressure
%
%  See also EX_STOKES_BIFURCATION_2D_RT_MP for an example
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2010, 2011, 2015, 2020 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [geometry, msh, space_v, vel, space_p, press] = ...
              mp_solve_stokes_div_conforming (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

if (strcmpi ('element_name', 'TH') || strcmpi ('element_name', 'SG'))
  error ('For TH and SG spaces, use mp_solve_stokes');
end

% Construct geometry structure, and information for interfaces and boundaries
[geometry, boundaries, interfaces, ~, boundary_interfaces] = mp_geo_load (geo_name);
npatch = numel (geometry);

msh = cell (1, npatch);
spv = cell (1, npatch);
spp = cell (1, npatch);
for iptc = 1:npatch
% Construct msh structure using the finest mesh
  msh_breaks = msh_set_breaks (element_name, ...
                                       geometry(iptc).nurbs.knots, nsub);
  rule      = msh_gauss_nodes (nquad);
  [qn, qw]  = msh_set_quad_nodes (msh_breaks, rule);
  msh{iptc} = msh_cartesian (msh_breaks, qn, qw, geometry(iptc));

% Construct space structure
  [spv{iptc}, spp{iptc}] = sp_bspline_fluid (element_name, ...
               geometry(iptc).nurbs.knots, nsub, degree, regularity, msh{iptc});
end

msh = msh_multipatch (msh, boundaries);
space_v = sp_multipatch (spv, msh, interfaces, boundary_interfaces);
space_p = sp_multipatch (spp, msh, interfaces, boundary_interfaces);
clear spv spp

% Compute and assemble the matrices
if (msh.rdim == 2)
  fun_one = @(x, y) ones (size(x));
elseif (msh.rdim == 3)
  fun_one = @(x, y, z) ones (size(x));
end

A = op_gradu_gradv_mp (space_v, space_v, msh, viscosity);
B = op_div_v_q_mp (space_v, space_p, msh);
E = (op_f_v_mp (space_p, msh, fun_one)).';
F = op_f_v_mp (space_v, msh, f);

vel   = zeros (space_v.ndof, 1);
press = zeros (space_p.ndof, 1);

% Apply DG techniques on the interfaces
A = A + mp_dg_penalty (space_v, msh, interfaces, viscosity, Cpen);

% Apply Neumann boundary conditions
rhs_nmnn = zeros(space_v.ndof,1);
Nbnd = cumsum ([0, boundaries.nsides]);
for iref = nmnn_sides
  iref_patch_list = Nbnd(iref)+1:Nbnd(iref+1);
  gref = @(varargin) g(varargin{:},iref);

  for bnd_side = 1:msh.boundaries(iref).nsides
    iptc = msh.boundaries(iref).patches(bnd_side);
    iside = msh.boundaries(iref).faces(bnd_side);

    msh_side = msh_eval_boundary_side (msh.msh_patch{iptc}, iside);
    msh_side_from_interior = msh_boundary_side_from_interior (msh.msh_patch{iptc}, iside);

    sp_bnd = space_v.sp_patch{iptc}.constructor (msh_side_from_interior);
    sp_bnd = sp_precompute (sp_bnd, msh_side_from_interior, 'value', true);
%     sp_bnd.dofs = 1:sp_bnd.ndof;

    x = cell (msh_side.rdim, 1);
    for idim = 1:msh_side.rdim
      x{idim} = reshape (msh_side.geo_map(idim,:,:), msh_side.nqn, msh_side.nel);
    end
    gval = reshape (gref(x{:}), msh.rdim, msh_side.nqn, msh_side.nel);
    rhs_nmnn(space_v.gnum{iptc}) = rhs_nmnn(space_v.gnum{iptc}) + ...
      op_f_v (sp_bnd, msh_side, gval);
  end
end

% Apply Dirichlet boundary conditions
[N_mat, N_rhs] = sp_weak_drchlt_bc_stokes (space_v, msh, drchlt_sides, h, viscosity, Cpen);
A = A - N_mat; F = F + N_rhs;
[vel_drchlt, drchlt_dofs] = sp_drchlt_l2_proj_udotn (space_v, msh, drchlt_sides, h);
vel(drchlt_dofs) = vel_drchlt;

int_dofs = setdiff (1:space_v.ndof, drchlt_dofs);
nintdofs = numel (int_dofs);
rhs_dir  = -A(int_dofs, drchlt_dofs)*vel(drchlt_dofs);

% Solve the linear system
if (isempty (nmnn_sides))
  mat = [A(int_dofs, int_dofs), -B(:,int_dofs).', sparse(nintdofs, 1);
         -B(:,int_dofs),        sparse(space_p.ndof, space_p.ndof), E.';
         sparse(1, nintdofs),   E, 0];
  rhs = [F(int_dofs) + rhs_dir;
         B(:, drchlt_dofs)*vel(drchlt_dofs);
         0];
  sol = mat \ rhs;
  vel(int_dofs) = sol(1:nintdofs);
  press = sol(1+nintdofs:end-1);
else
% With natural boundary condition, the constraint on the pressure is not needed.
  mat = [ A(int_dofs, int_dofs), -B(:,int_dofs).';
         -B(:,int_dofs),         sparse(size (B,1), size (B,1))];
  rhs = [F(int_dofs) + rhs_dir + rhs_nmnn(int_dofs);
         B(:, drchlt_dofs)*vel(drchlt_dofs)];
  sol = mat \ rhs;
  vel(int_dofs) = sol(1:nintdofs);
  press = sol(1+nintdofs:end);
end

end


function [f,g,H] = autoHess(x,useComplex,funObj,varargin)
% Numerically compute Hessian of objective function from gradient values

p = length(x);

if useComplex % Use Complex Differentials
    mu = 1e-150;

    diff = zeros(p);
    for j = 1:p
        e_j = zeros(p,1);
        e_j(j) = 1;
        [f(j) diff(:,j)] = funObj(x + mu*i*e_j,varargin{:});
    end
    f = mean(real(f));
    g = mean(real(diff),2);
    H = imag(diff)/mu;
else % Use finite differencing
    mu = 2*sqrt(1e-12)*(1+norm(x))/norm(p);

    [f,g] = funObj(x,varargin{:});
    diff = zeros(p);
    for j = 1:p
        e_j = zeros(p,1);
        e_j(j) = 1;
        [f diff(:,j)] = funObj(x + mu*e_j,varargin{:});
    end
    H = (diff-repmat(g,[1 p]))/mu;
end

% Make sure H is symmetric
H = (H+H')/2;

if 0 % DEBUG CODE
    [fReal gReal HReal] = funObj(x,varargin{:});
    [fReal f]
    [gReal g]
    [HReal H]
    pause;
end


% SIMPLE ALGORITHM TO FOLLOW A LINE IN SPACE.
% Copyright (C) 2019, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function speed_demo_line
close all
% velocidad lineal entre puntos consecutivos
abs_linear_speed = 0.5; % (m/s)
delta_time = 0.05;

robot = load_robot('ABB', 'IRB52')

fprintf('\nPRESS ANY KEY TO CONTINUE...')
pause

%NOA matrix initial point
T1=[1 0 0 0.8;
    0 1 0 -0.3;
    0 0 1 0.9;
    0 0 0  1]
%NOA matrix end point
T2=[1 0 0 0.5;
    0 1 0 0.4;
    0 0 1 0.7;
    0 0 0  1]

punto_inicial = T1(1:3,4);
punto_final = T2(1:3,4);
% vector velocidad en la direcci�n de la trayectoria
v = (punto_final-punto_inicial);
v = abs_linear_speed*v/norm(v); %vector normalizado en la direcci�n de la recta
w = [0 0 0]';
xd = [v; w];

qinv = inversekinematic(robot, T1);

%Select arbitrarily the first solution
q = qinv(:,1);
qt = [q];
for i=1:1000
   J = manipulator_jacobian(robot, q);
   qd = inv(J)*xd;
   q = q + delta_time*qd;
   qt = [qt q];
end
animate(robot, qt)


function ierror = perm_check2 ( n, p, base )

%*****************************************************************************80
%
%% PERM_CHECK2 checks that a vector represents a permutation.
%
%  Discussion:
%
%    The routine verifies that each of the integers from BASE to
%    to BASE+N-1 occurs among the N entries of the permutation.
%
%    Set the input quantity BASE to 0, if P is a 0-based permutation,
%    or to 1 if P is a 1-based permutation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries.
%
%    Input, integer P(N), the array to check.
%
%    Input, integer BASE, the index base.
%
%    Output, integer IERROR, error flag.
%    0, the array represents a permutation.
%    nonzero, the array does not represent a permutation.
%
  ierror = 0;

  for seek = base : base + n - 1

    ierror = 1;

    for find = 1 : n
      if ( p(find) == seek )
        ierror = 0;
        break;
      end
    end

    if ( ierror ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PERM_CHECK2 - Fatal error!\n' );
      fprintf ( 1, '  The input array does not represent\n' );
      fprintf ( 1, '  a proper permutation.\n' );
      error ( 'PERM_CHECK2 - Fatal error!' );
      return
    end

  end

  return
end


function totalRCS = RCS_Compute(complex_data, oversample_ratio, cal_sf, mask)
%RCS_COMPUTE Demonstrates area-based computation of calibrated RCS for an ROI
%
% USAGE:
%   totalRCS = RCS_Compute(complex_data, oversample_ratio, cal_sf, mask)
%
% INPUTS:
%   complex_data     - required : Complex valued SAR dataset in the image
%                                 domain.  First dimension is azimuth,
%                                 second range.  Third dimension could be
%                                 for multi-channel (i.e. polarimetric)
%                                 data.  This code assumes the frequency
%                                 support is centered and constant across
%                                 the image.
%   oversample_ratio - optional : Overample or zeropad factor. (Required
%                                 for calibrated RCS.)
%   cal_sf           - optional : Calibration scale factor (linear).
%                                 Either a constant or an array the same
%                                 size as complex_data with per-pixel
%                                 values. (Required for calibrated RCS.)
%   mask             - optional : Binary image which is ones over the
%                                 region of interest. (Default is an image
%                                 of all ones.)
%
% OUTPUTS:
%   totalRCS         - required : Total calibrated RCS of ROI (calibrated
%                                    if oversample_ratio and cal_sf were
%                                    given.)
%
% Author: Tim Cox, NRL; Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Default parameter values
if ~exist('oversample_ratio','var')
    oversample_ratio = [1 1]; % RCS values will be uncalibrated if this is unknown
end
if ~exist('cal_sf','var')
    cal_sf = 1; % RCS values will be uncalibrated if this is unknown
end
if ~exist('mask','var')
    mask = ones(size(complex_data));
end

% Apply shape mask to rectangular data
filtimg = complex_data.*repmat(mask,[1 1 size(complex_data,3)]);
if ~isscalar(cal_sf)
    % Only works if the radiometric scale factors are same for both channels
    cal_sf = repmat(cal_sf,[1 1 size(complex_data,3)]);
end

% Image domain computation of total RCS (Reference Adam Bryant, NGA/PL)
totalRCS = (1/prod(oversample_ratio)) * sum(sum(cal_sf.*abs(filtimg).^2));
totalRCS = squeeze(totalRCS); % Multi-channel (polarimetric) data
% The oversample ratio (or zeropad) factor is the ratio between the sum of
% squared (power detected) samples of an ideal sinc function (which is what
% we are measuring, at least for an ROI which we assume contains nearly all
% the energy of that sinc) and the peak of that ideal sinc^2 function
% (which is how the calibration constant is defined).
% We put the scale factor inside the sum so that it can be applied per
% pixel, rather than only as a constant.

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [params] = gui_CalcOmori(params,nNodeStart, nNodeEnd)
    % gui_CalcOmori Calculate Omori parameters, here setting up the data
    %
    % [params] = gui_CalcOmori(params,nNodeStart, nNodeEnd);
    % ----------------------------------------------------------------
    %
    % Input parameters:
    %   params.mCatalog           Earthquake catalog
    %   params.mPolygon           Polygon (defined by ex_selectgrid)
    %   params.vX                 X-vector (defined by ex_selectgrid)
    %   params.vY                 Y-vector (defined by ex_selectgrid)
    %   params.vUsedNodes         Used nodes vX * vY defining the mPolygon (defined by ex_selectgrid)
    %   params.nCalculation       Number of random simulations
    %   params.bMap               Calculate a map (true) or a cross-section (false)
    %   params.bNumber            Use constant number (true) or constant radius (false)
    %   params.nNumberEvents      Number of earthquakes if bNumber is true
    %   params.fMaxRadius         Maximum Radius using a constant number of events; works only with bNumber is true
    %   params.fRadius            Radius of gridnode if bNumber is false
    %   params.nMinimumNumber     Minimum number of earthquakes per node for determining a b-value
    %   params.fMinMag            Lower limit of magnitude range for testing
    %   params.fMaxMag            Upper limit of magnitude range for testing
    %   params.bTimePeriod        Calculate seismicity difference for 2 periods (0) until start and end of catalog or
    %                             a specific time period before and after fSplitTime (1)
    %   params.fTimePeriodDays        Length of time periods
    %   params.bTstart            Check for starting time of temporal mapping
    %   params.fTstart            Starting time for temporal mapping
    %   params.bBstnum            Check for boostrap sampling
    %   params.fBstnum            Number of bootstrap samples
    %   params.fBinning           Bin size for magnitude binning
    %   params.sComment           Comment on calculation

    % Output parameters:
    %   Same as input parameters including
    %   params.mValueGrid         Matrix of calculated values
    %   params.vcsGridNames       Names of parameters calculated
    %   Check gui_NodeCalcOmori.m for a list of variables!!
    %
    % J. Woessner; j.woessner@sed.ethz.ch
    % updated: 16.02.2006

    report_this_filefun();

    % Check calculation for splitting of nodes
    if nargin < 2
        nNodeStart = 1;
        nNodeEnd = length(params.mPolygon(:,1));
    end

    % Initialize
    vResults = [];
    params.sComment = [];
    if isempty(params.fBinning)
        params.fBinning = 0.1;
    end

    % Determine time period of catalog
    params.fTminCat = min(params.mCatalog.Date);
    params.fTmaxCat = max(params.mCatalog.Date);

    % Init result matrix
    mValueGrid_ = [];

    % Temporary saving the original catalog
    mCatalog = params.mCatalog;

    % Force saving all 100 nodes
    fDivide = length(params.mPolygon(:,1))/100
    fForceSave = length(params.mPolygon(:,1))/fDivide;

    % % Loop over time
    fTstart = params.fTstart;
    % while fTstart < params.fTmaxCat
    % mValueGrid_ = [];
    % params.mCatalog = mCatalog;

    % Create Indices to catalog
    [params.caNodeIndices] = ex_CreateIndexCatalog(params.mCatalog, params.mPolygon, params.bMap, params.nGriddingMode, ...
        params.nNumberEvents, params.fRadius, params.fSizeRectHorizontal, params.fSizeRectDepth);
    % Loop over all grid nodes
    hWaitbar1 = waitbar(0,'Calculating nodes...');
    set(hWaitbar1,'Numbertitle','off','Name','Node percentage');
    for nNode_ = nNodeStart:nNodeEnd
        % Create node catalog
        mNodeCatalog_ = params.mCatalog(params.caNodeIndices{nNode_}, :);
        % Check for constant number of events calculations
        if (params.nGriddingMode == 0)
            [mNodeCatalog_] = ex_CheckMaxRadius(mNodeCatalog_, params.mPolygon, nNode_, params.caNodeIndices, params.fMaxRadius, params.nNumberEvents, params.bMap);
        end
        % Check for number of events in aftershock sequence
        % Create catalog after split time (aftershock sequence)
        vSelAf = (params.fTstart <= mNodeCatalog_(:,3) & mNodeCatalog_(:,3) < params.fTstart+params.fTimePeriodDays);
        [nX,nY] = size(mNodeCatalog_(vSelAf,:));
        if (nX < params.nMinimumNumber)
            [nNumBgr,nY2] = size(mNodeCatalog_(~vSelAf,:));
            mValueGrid_= [mValueGrid_; NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN nNumBgr nNumBgr/params.fTimeBgr NaN NaN];
        else
            [rCalcNodeResult_] = gui_NodeCalcOmori(params,mNodeCatalog_);
            % Store the results
            mValueGrid_= [mValueGrid_;rCalcNodeResult_.pval1 rCalcNodeResult_.cval1 rCalcNodeResult_.kval1 rCalcNodeResult_.nNumEvents rCalcNodeResult_.fLogEqdens...
                rCalcNodeResult_.H rCalcNodeResult_.pmean1 rCalcNodeResult_.cmean1 rCalcNodeResult_.kmean1...
                rCalcNodeResult_.pmeanStd1 rCalcNodeResult_.cmeanStd1 rCalcNodeResult_.kmeanStd1...
                rCalcNodeResult_.fTafseq rCalcNodeResult_.fTafseqmean rCalcNodeResult_.nNumAf rCalcNodeResult_.nNumBgr...
                rCalcNodeResult_.fBgrate rCalcNodeResult_.fLog10Bgrate rCalcNodeResult_.fLog10Tafseq];
        end % End of if on nNode_
        if rem(nNode_,floor(fForceSave)) == 0
            waitbar(nNode_/length(params.mPolygon(:,1)))
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Temporary saving
            params.vcsGridNames = cellstr(char('p-value','c-value','k-value','Number of events','log(EQ_density)',...
                'H','p-mean','c-mean','k-mean','\sigma (p)','\sigma (c)','\sigma (k)','t_a [years]','t_a(Bst) [years]',...
                'Number of aftershock','Num. events background','Background rate [year]','Log10(Bgr rate) [year]',...
                'log10(t_a) [years]'));
            params.mValueGrid = mValueGrid_;
            % Add parameter to params.sComment
            if  params.nGriddingMode == 0;   % Constant number
                params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
                    num2str(params.fMaxRadius) ' km'];
                vResults = params;
                save(['tmp_result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
                    '_Nmin_' num2str(params.nMinimumNumber) '_Node' num2str(nNode_) '.mat'], 'vResults');
            elseif params.nGriddingMode == 1;   % Constant radius
                params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
                vResults = params;
                save(['tmp_result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
            else  % Rectangle mode
                params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
                    ' km, Nmin: ' num2str(params.nMinimumNumber)];
                vResults = params;
                save(['tmp_result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
                    '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
            end % END of params.nGriddingmode
            vResults =[];
        end % End updating waitbar
    end % for nNode
    close(hWaitbar1);
    % Parameter description
    params.vcsGridNames = cellstr(char('p-value','c-value','k-value','Number of events','log(EQ_density)',...
        'H','p-mean','c-mean','k-mean','\sigma (p)','\sigma (c)','\sigma (k)','t_a [years]','t_a(Bst) [years]',...
        'Number of aftershock','Num. events background','Background rate [year]','Log10(Bgr rate) [year]',...
        'log10(t_a) [years]'));
    params.mValueGrid = mValueGrid_;
    % Add parameter to params.sComment
    if  params.nGriddingMode == 0;   % Constant number
        params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
            ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
            num2str(params.fMaxRadius) ' km'];
        vResults = params;
        save(['result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
            '_Nmin_' num2str(params.nMinimumNumber) '_Nodes_' num2str(nNodeStart) '_' num2str(nNodeEnd) '.mat'], 'vResults');
    elseif params.nGriddingMode == 1;   % Constant radius
        params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
            ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
        vResults = params;
        save(['result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber)...
            '_Nodes_' num2str(nNodeStart) '_' num2str(nNodeEnd) '.mat'], 'vResults');
    else  % Rectangle mode
        params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
            ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
            ' km, Nmin: ' num2str(params.nMinimumNumber)];
        vResults = params;
        save(['result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
            '_Nmin_' num2str(params.nMinimumNumber) '_Nodes_' num2str(nNodeStart) '_' num2str(nNodeEnd) '.mat'], 'vResults');
    end


function ll = gpsimCandidateLogLikelihood(model)

% GPSIMCANDIDATELOGLIKELIHOOD Compute the log likelihood of a gene.
% FORMAT
% DESC computes the log likelihood of one gene given the existing genes
% in the GPSIM model. For use when ranking other prospective candidate genes.
% ARG model : the model containing the existing genes for which the log likelihood is computed.
% RETURN ll : the log likelihood of the new gene.
%
% SEEALSO : gpsimCreate, gpsimAddCandidate, gpsimCandidateLogLikelihood,
% gpsimCandidateObjective
%
% COPYRIGHT : Neil D. Lawrence, 2007

% SHEFFIELDML

dim = size(model.y, 1);

invK_ffm = model.candidate.invK*model.candidate.m;
K_ufinvK_ffm = model.candidate.K_uf*invK_ffm;

ll = -0.5*(dim*log(2*pi) + model.candidate.logDetK ...
           + model.candidate.logDetA ...
           - model.logDetK ...
           + sum(sum(invK_ffm.*model.candidate.m))...
           - sum(sum((model.candidate.Ainv*K_ufinvK_ffm).*K_ufinvK_ffm)));

% In case we need priors in.
if isfield(model.candidate, 'bprior'),
  ll = ll + kernPriorLogProb(model.candidate.kern);
  ll = ll + priorLogProb(model.candidate.bprior, model.B);
end


function imsegs = APPimages2superpixels(input_dir, ext, gt)
% imsegs = APPimages2superpixels(input_dir, ext, gt)
% Create the imsegs structure from a segmentation image
%
% INPUT:
% input_dir - The directory of segmentation images. Segments are denoted by
% different colors.
% ext - the extension of the segmentation image filenames
% gt - an existing imsegs structure (or [] if none exists)
% OUTPUT:
% imsegs - image segmentation data (and sometimes ground truth)
%
% Example: imsegs = APPimages2superpixels('../images', 'pnm', [])
%
% Copyright(C) Derek Hoiem, Carnegie Mellon University, 2005
% Permission granted to non-commercial enterprises for
% modification/redistribution under GNU GPL.
% Current Version: 1.0  09/30/2005

if isempty(gt)
    files = dir([input_dir '/*.' ext]);
    for f = 1:length(files)
        gt(f).image_name = files(f).name;
    end
end

imsegs(length(gt)) = struct('imname', '', 'imsize', [0 0]);
for f = 1:length(gt)
    imname = gt(f).image_name;
    disp(imname)
    basename = strtok(imname, '.');
    im = imread([input_dir '/' basename '.' ext]);

    im = double(im);

    imsegs(f).imname = [strtok(imname,'.') '.jpg'];
    imsegs(f).imsize = size(im);
    imsegs(f).imsize = imsegs(f).imsize(1:2);
    im = im(:, :, 1) + im(:, :, 2)*256 + im(:, :, 3)*256^2;
    [gid, gn] = grp2idx(im);
    imsegs(f).segimage = uint16(reshape(gid, imsegs(f).imsize));
    imsegs(f).nseg = length(gn);

end
imsegs = APPgetSpStats(imsegs);


function gX = velotransKernGradX(kern, X, X2)

% VELOTRANSKERNGRADX Gradient of VELOTRANS kernel with respect to a point x.
% FORMAT
% DESC computes the gradient of the velocity translate
% kernel with respect to the input positions.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x : locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData x numInputs x numData. Where numData is
% the number of data points and numInputs is the number of input
% dimensions in X.
%
% FORMAT
% DESC computes the gradident of the velocity translate
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x1 : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO velotransKernParamInit, kernGradX, velotransKernDiagGradX, translateKernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2011

% KERN

  t = X(:, end);
  xPass = X(:, 1:end-1) - t*kern.velocity;
  t2 = X2(:, end);
  x2Pass = X2(:, 1:end-1) - t2*kern.velocity;
  gXRecover = cmpndKernGradX(kern, xPass, x2Pass);
  gX = zeros(size(X2, 1), size(X2, 2), size(X, 1));
  for i = 1:size(X, 1)
    gX(:, 1:end-1, i) = gXRecover(:, :, i);
    gX(:, end, i) = -gXRecover(:, :, i)*kern.velocity'; % Another gradient
                                                        % guess
  end
end


function xoverKids  = crossoverNcK(parents,options,GenomeLength,FitnessFcn,unused,thisPopulation)
% Oren Rosen
% The MathWorks
% 8/29/2007
%
% This custom crossover function is written to work on a population of
% vectors of zeros and ones with the same amount of ones in each vector.
% The children that are produced from 2 parents will have the same genes
% for every element they agree on, and random choices of zerps and ones for
% the elements they don't agree on. The number of each is set so that all
% children have the same number of ones as their parents. All children
% automatically satisfy this constraint so there is no need to impose these
% constraints.

% How many children to produce?
nKids = length(parents)/2;

% Allocate space for the kids
xoverKids = zeros(nKids,GenomeLength);

% To move through the parents twice as fast as thekids are
% being produced, a separate index for the parents is needed
index = 1;

% *** Initialize ***
% Assumes all members of thisPopulation have the same number of ones.
num1s = sum(thisPopulation(1,:));
indexVec = 1:GenomeLength;

% for each kid...
for i=1:nKids

    % *** Get parents ***
    r1 = parents(index);
    index = index + 1;
    r2 = parents(index);
    index = index + 1;

    p1 = thisPopulation(r1,:);
    p2 = thisPopulation(r2,:);

    % *** Find Matching 1's and 0's ***
    % Ex: If           p1 == [ 1 0 1 0 0 1 1 0 0 0 ]
    %                  p2 == [ 1 0 0 1 0 0 1 0 1 0 ]
    %     Then matching1s == [ 1 0 0 0 0 0 1 0 0 0 ]
    %     Then matching0s == [ 0 1 0 0 1 0 0 1 0 1 ]
    matching1s = ~xor(p1,p2) & (p1 == 1);
    matching0s = ~xor(p1,p2) & (p1 == 0);

    % *** Find Matching Indices ***
    %     If       matching1s == [ 1 0 0 0 0 0 1 0 0 0 ]
    %              matching0s == [ 0 1 0 0 1 0 0 1 0 1 ]
    %     Then matching1sIndx == [ 1 7 ]
    %          matching0sIndx == [ 2 5 8 10 ]
    %         nonmatchingIndx == [ 3 4 6 9 ]
    matching1sIndx = indexVec(matching1s);
    matching0sIndx = indexVec(matching0s);
    nonmatchingIndx = setdiff(indexVec,[matching0sIndx,matching1sIndx]);

    % *** Create Child ***
    % Ex: If   num1s == 4
    %          matching1sIndx == [ 1 7 ]
    %          nonmatchingIndx == [ 3 4 6 9 ]
    %     Then numMatching1s == 2
    %     num1sToFill == 2
    %     Indx1sToFill == 2 random choices from [ 3 4 6 9 ]
    numMatching1s = numel(matching1sIndx);
    num1sToFill = num1s - numMatching1s;
    Indx1sToFill = randsample(nonmatchingIndx,num1sToFill);

    % *** Fill in 1s ***
    % Ex: If               p1 == [ 1 0 1 0 0 1 1 0 0 0 ]
    %                      p2 == [ 1 0 0 1 0 0 1 0 1 0 ]
    %     Then xoverKids(i,:) == [ 1 0 ? ? 0 ? 1 0 ? 0 ]
    %     With exactly 2 of the '?' equal to 1, the rest 0.
    xoverKids(i,matching1sIndx) = 1;
    xoverKids(i,Indx1sToFill) = 1;
end


%RECALL   query the current discretized problem in the workspace
%IFISS scriptfile: DJS; 27 April 2012.
%Copyright (c) 2009 D.J. Silvester, H.C. Elman, A. Ramage
%
%For more help, type 'helpme'.
if exist('pde','var')==0,
error('No discrete problem found in the workspace!'),
end
%
if pde>10 & pde <20, fprintf(' Unsteady solution\n'), pdex=pde-10;
else, pdex=pde; end
%
if pdex<3,
fprintf(' scalar problem       : domain       : approximation')
     if pdex==1, fprintf('\n Poisson Equation     :'),
 elseif pdex==2, fprintf('\n Convection-Diffusion :'),
    end
    if domain==1, fprintf(' square       :'),
elseif domain==2, fprintf(' L-shaped     :'),
            else, fprintf(' non-standard :'),
    end
    if qmethod==1, fprintf(' Q1\n'),
elseif qmethod==2, fprintf(' Q2\n'),
			else,  fprintf(' non-standard\n'),
    end
else
fprintf(' flow problem  :        geometry          : approximation')
    if pdex==3, fprintf('\n Stokes        :'),
elseif pdex==4, fprintf('\n Navier-Stokes :'),
elseif pdex==5, fprintf('\n Boussinesq    :'),
    end
	if domain==1,  fprintf(' enclosed square          :'),
elseif domain==3,  fprintf(' step with inflow/outflow :'),
elseif domain==4,  fprintf(' channel with obstacle    :'),
elseif domain==7,  fprintf(' enclosed cavity          :'),
elseif domain==10, fprintf(' simple channel           :'),
	         else, fprintf(' non-standard'),
	end
	if qmethod==0, fprintf(' Q1-Q1\n'),
elseif qmethod==1, fprintf(' Q1-P0\n'),
elseif qmethod==2, fprintf(' Q2-Q1\n'),
elseif qmethod==3, fprintf(' Q2-P1\n'),
elseif qmethod==12, fprintf(' Q2-Q1-Q2\n'),
else, fprintf('non-standard\n'),
    end
end


function [P]=patchSmooth(F,V,IND_V,cPar)

% function [P]=patchSmooth(F,V,IND_V,cPar)
% ------------------------------------------------------------------------
%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 2014/06/02
%------------------------------------------------------------------------

%%

%Get/set method
if isfield(cPar,'Method')
    smoothMethod=cPar.Method;
else
    smoothMethod='LAP'; %DEFAULT
end

if ~isfield(cPar,'n')
    cPar.n=1; %DEFAULT
end

if isempty(IND_V)
    [~,IND_V]=patchIND(F,V,2);
%     if isa(F,'cell')
%         IND_V=[];
%         for q=1:1:numel(F)
%             C=patchConnectivity(F{q},V,'vv');
%             IND_V=[IND_V C.vertex.vertex];
%         end
%     else
%         C=patchConnectivity(F,V,'vv');
%         IND_V=C.vertex.vertex;
%     end

end

%Smooth
switch smoothMethod
    case 'LAP' %Laplacian
        [P]=tesSmooth_LAP(F,V,IND_V,cPar);
    case 'HC' %Humphreys Classes
        [P]=tesSmooth_HC(F,V,IND_V,cPar);
    case 'tLAP' %Tangent Laplacian

        %Invert face orientation if required
        [logicFlip]=isGlobalSurfDirOutward(F,V);
        if ~logicFlip
            F=fliplr(F); %Flip faces
        end

        %Set control parameters for Laplacian smoothening iterations
        cPar_t=cPar;
        cPar_t.n=1;
        cPar_t.Tolerance=[];
        P=V;
        for q=1:1:cPar.n
            [Ps]=tesSmooth_LAP(F,P,IND_V,cPar_t);  %The Laplacian smoothened coordinate set
            D=Ps-P; %smoothening intended displacement vectors
            [Dt]=patchVectorTangent(F,P,D,[]); %Tangential component of displacement
            P=P+Dt; %Displace mesh
        end

    case 'tHC' %Tangent HC

        %Invert face orientation if required
        [logicFlip]=isGlobalSurfDirOutward(F,V);
        if ~logicFlip
            F=fliplr(F); %Flip faces
        end

        %Set control parameters for Laplacian smoothening iterations
        cPar_t=cPar;
        cPar_t.n=1;
        cPar_t.Tolerance=[];
        P=V;
        for q=1:1:cPar.n
            [Ps]=tesSmooth_HC(F,P,IND_V,cPar_t);  %The smoothened coordinate set
            D=Ps-P; %smoothening intended displacement vectors
            [Dt]=patchVectorTangent(F,P,D,[]); %Tangential component of displacement
            P=P+Dt; %Displace mesh
        end

    otherwise
        error('Invalid smooth method specified');
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function test_failed=test_zak

disp(' ===============  TEST_ZAK ================');

Lr=[1,12,12,12,12,12,15,15,15];
Kr=[1, 1, 2, 3, 4, 6, 3, 5,15];

ref_failed=0;
inv_failed=0;

W=1;

for ii=1:length(Lr)

  L=Lr(ii);
  K=Kr(ii);

  f=tester_rand(L,W)+i*rand(L,W)-.5-i*.5;

  ccref=ref_zak(f,K);
  cc=zak(f,K);
  r=izak(cc);

  res=ccref-cc;

  nres=norm(res(:));
  ninv=norm(f-r);

  s=sprintf('REF L:%3i K:%3i %0.5g',L,K,nres);
  disp(s)

  [ref_failed,fail]=ltfatdiditfail(nres,ref_failed);
  % if nres>10e-10
    % disp('FAILED');
    % ref_failed=ref_failed+1;
  % end;

  s=sprintf('INV L:%3i K:%3i %0.5g',L,K,ninv);
  disp(s)

  [inv_failed,fail]=ltfatdiditfail(ninv,inv_failed);
  % if nres>10e-10
    % disp('FAILED');
    % inv_failed=inv_failed+1;
  % end;

end;

test_failed=ref_failed+inv_failed;


function varargout = max(varargin)
%MAX   Maximum value of a CHEBFUN2 in one direction.
%   MAX(f) returns a chebfun representing the maximum of the CHEBFUN2 along the
%   y direction, i.e, MAX(f) = @(x) max( f ( x, : ) )
%
%   MAX(f, [], dim) returns a CHEBFUN representing the maximum of f along the
%   DIM direction. If DIM = 1 is along the y-direction and DIM = 2 is along the
%   x-direction.
%
%   WARNING: This function is not always accurate to the expected precision.
%
%   For the global maximum use MAX2.
%
% See also MIN, MAX2, MIN2, MINANDMAX2.

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

[varargout{1:nargout}] = max@separableApprox(varargin{:});

end


function displayInterRes( imgIn )
%DISPLAYINTERRES display intermediate results
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

% set all parameters here inside

persistent paraIR;

if(ischar(imgIn))
    clear paraIR;
    return;
end

if(~isfield(paraIR,'dim'))
    paraIR.dim = ndims(imgIn);
    if(paraIR.dim == 3)
        % y-x-cha
        paraIR.subs = {':',':',':'};
        paraIR.permRule = [];

    elseif(paraIR.dim == 4)
        % 3D: y-z-x-cha

        paraIR.subs = {':', ':', ceil(size(imgIn,2)/2), ':'};
        paraIR.permRule = [1 3 2 4]; % => y-x-z-cha

%         % OR
%         % 2Dt: t-y-x-cha
%         paraIR.subs = {':', ':', ceil(size(imgIn,1)/2), ':'};
%         paraIR.permRule = [2 3 1 4]; % => y-x-t-cha

    elseif(paraIR.dim == 5)
        % t-y-z-x-cha
        paraIR.subs = {':', ':', ceil(size(imgIn,3)/2), ceil(size(imgIn,1)/2), ':'};
        paraIR.permRule = [2 4 3 1 5]; % => y-x-z-t-cha
    end

    paraIR.range = [0 1];
    paraIR.iter = 1;
    paraIR.normF = [];
    paraIR.screensize = get(0,'ScreenSize');

    paraIR.plotNorm = false;
end

if(~isempty(paraIR.permRule))
    imgIn = permute(imgIn,paraIR.permRule);
end
imgIn = imgIn(paraIR.subs{:});
imgIn = ((imgIn - min(imgIn(:))) * (paraIR.range(2)-paraIR.range(1)))./(max(imgIn(:)) - min(imgIn(:)));

% show channel combined abs image
imgIn = sqrt(sum(abs(imgIn).^2, ndims(imgIn)));
paraIR.normF(end+1) = norm(imgIn(:));

paraIR.hfig = figure(999);
set(paraIR.hfig,'position',[paraIR.screensize(1)+5 paraIR.screensize(2)+50 paraIR.screensize(3)-10 paraIR.screensize(4)-130]);
if(paraIR.plotNorm)
    subplot(2,1,1);
end
imagesc( imgIn, paraIR.range );
title(sprintf('\\bfiteration %g \nFrobenius norm: %g', paraIR.iter, norm(imgIn(:))))
axis image
colormap(gray(256))
% drawnow
if(paraIR.plotNorm)
    subplot(2,1,2);
    plot(paraIR.normF);
    xlabel('Iteration');
    ylabel('Frobenius norm');
    title('Frobenius norm behaviour');
end
drawnow
pause(0.5);

paraIR.iter = paraIR.iter + 1;

end


function h = plot_correlation(X,Y,varargin)
% :Usage:
% ::
%
%    handles = plot_correlation(X,Y,varargin)
%
% plots robust or OLS simple or partial correlations
% replaces prplot and plot_correlation_samefig. *Remove NaNs from X and Y
% before calling this function.*
%
% :Inputs:
%
%   **X:**
%        is matrix of columns of interest plus nuisance
%        default is to plot partial effect of 1st column
%
%   **Y:**
%        is one or more columns of data
%
% :Optional Inputs:
%
%   **'robust':**
%        robust IRLS plot
%
%   **'noprint':**
%        suppress text output
%
%   **'doquad':**
%        quadratic term; not tested, may not work
%
%   **'col':**
%        followed by column of interest
%
%   **'labels':**
%        followed by cell array of text labels for each obs.
%
%   **'colors':**
%        followed by cell array of colors for each column of Y
%
%   **'ylabel':**
%        followed by y-axis label string
%
%   **'xlabel':**
%        followed by x-axis label string
%
%   **'weights':**
%        followed by weights that override any computed ones
%
% :Examples: Plot robust partial corr. 2 of X against col. 17 of Y,
% controlling for other X
% ::
%
%    figure;
%    h = plot_correlation(X,Y(:,17),'col',2,'robust','ylabel','Brain
%                         data','xlabel','Order effect');
%
% Plot Col. 1 of X vs. Y in red squares
% ::
%
%    figure;
%    h = plot_correlation(X,Y(:,17),'robust','colors',{'rs'});
%
%    tor_fig;
%
% ..
%    tor wager, august 2006
% ..

% ---------------------------------------------
% default behaviors
% ---------------------------------------------
dorobust = 0;
doprint = 1;
wh_interest = 1;
doquad = 0;
mylabels = [];
mycol = {'ko' 'rv' 'bs' 'gd' 'y^' 'cv' 'mx'};
ylabelstr = 'Contrast beta value';
xlabelstr = 'Behavioral score';

% ---------------------------------------------
% Optional inputs
% ---------------------------------------------

for i = 1:length(varargin)
    if isstr(varargin{i})
        switch varargin{i}
            % reserved keywords
            case 'robust', dorobust = 1;
            case 'noprint', doprint = 0;
            case 'doquad', doquad = 1;

            % functional commands
            case 'labels', labels = varargin{i+1};
            case 'colors', mycol = varargin{i+1};
            case 'ylabel', ylabelstr = varargin{i+1}; varargin{i+1} = [];
            case 'xlabel', xlabelstr = varargin{i+1}; varargin{i+1} = [];
            case 'col', wh_interest = varargin{i+1};
            case 'weights', myweights = varargin{i+1};
            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end
    end
end

% ---------------------------------------------
% drop NANs
% ---------------------------------------------
todrop = isnan(X) | isnan(Y);
if any(todrop)
    warning('Dropping %d observations due to NaNs', sum(todrop))
    X(todrop) = [];
    Y(todrop) = [];
end

% ---------------------------------------------
% intercept
% ---------------------------------------------
wh_intercept = find(all(diff(X) < eps));
if isempty(wh_intercept)
    wh_intercept = size(X,2) + 1;
    X(:,wh_intercept) = 1;
end

ny = size(Y,2);
while length(mycol) < ny, mycol = [mycol mycol]; end

for i = 1:ny
    [x,y,r,p,se,meany,stats] = partialcor(X,Y(:,i),wh_interest,doprint,dorobust);

    % b(1) is param, b(2) is intercept for partial plot
    b = stats.b([wh_interest wh_intercept]);

    % weights
    w = stats.w;
    if exist('myweights','var'), w = myweights; end

    h{i} = makefigure(x,y,mycol{i},mylabels,doquad,b,w,xlabelstr,ylabelstr);

    text(min(X(:,1)),max(y),sprintf('r = %3.2f',r),'FontSize',16,'Color',mycol{i}(1));

end

return

function h = makefigure(xvec,yvec,mycol,mylabels,doquad,b,varargin)
% h = makefigure(xvec,yvec,mycol,mylabels,doquad,b,[weights],[xlabel],[ylabel])

ylabelstr = 'Contrast beta value';
xlabelstr = 'Behavioral score';

hold on; grid on; set(gca,'FontSize',18)

if length(varargin) > 0
    % ROBUST (or we just have weights)
    w = varargin{1};
    for i = 1:length(xvec)

        if isnan(xvec(i)) | isnan(yvec(i)), i, continue, end

        h(i) = plot(xvec(i),yvec(i),mycol,'LineWidth',.5,'MarkerSize',8, ...
            'MarkerFaceColor',mycol(1));
        set(h(i),'MarkerFaceColor',[repmat(1-w(i),1,3)] )
    end

    % set axis
    xlims = [min(xvec) max(xvec)];
    ylims = [min(yvec) max(yvec)];
    xrange = (xlims(2) - xlims(1)) *.1; % % margin
    yrange = (ylims(2) - ylims(1)) *.1;
    xlims = xlims + [-xrange xrange];
    ylims = ylims + [-yrange yrange];

    set(gca,'Xlim',xlims,'YLim',ylims);

    % plot regression line by hand
    xl = get(gca,'Xlim'); yl = get(gca,'Ylim');
    x = xl(1)-5:xl(2)+5;
    h2 = plot(x,b(1) * x + b(2),'-','Color',mycol(1),'LineWidth',2);
    h = [h h2];
    set(gca,'XLim',xl,'YLim',yl)

else
    % not robust

    h = plot(xvec,yvec,mycol,'LineWidth',3,'MarkerSize',6,'MarkerFaceColor',mycol(1));

    if doquad
        %refcurve(doquad)
    else
        try
            refline
        catch
            tmp = get(gca,'XLim');
            x = min(xvec)-std(xvec):min(std(xvec),.01):max(xvec)+std(xvec);
            plot(x,b(1) * x + b(2),[mycol(1) '-'],'LineWidth',.5)
            set(gca,'XLim',tmp);

        end
    end

end

drawnow

if length(varargin) > 1
    xlabelstr = varargin{2};
end

if length(varargin) > 2
    ylabelstr = varargin{3};
end

if ~isempty(mylabels)
    for j = 1:length(xvec)
        text(xvec(j),yvec(j),mylabels{j},'FontWeight','b')
    end
end

ylabel(ylabelstr)
xlabel(xlabelstr)

return


filename='Bridge_hexahedra_coarse';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT'; incrementFactor = 1;
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.05;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;


function updatefly(h,x,y,theta,maj,min)

% draw an isosceles triangle with center (x,y)
% with rotation theta
% with height maj*4
% with base min*4

if isstruct(x),
  trx = x;
  t = y;
  x = trx.x(t);
  y = trx.y(t);
  theta = trx.theta(t);
  maj = trx.a(t);
  min = trx.b(t);
end

if 0,

  ellipseupdate(h,maj*2,min*2,x,y,theta);

else

% isosceles triangle not yet rotated or centered
pts = [-maj*2,-min*2,
       -maj*2,min*2,
       maj*2,0];

% rotate
costheta = cos(theta);
sintheta = sin(theta);
R = [costheta,sintheta;-sintheta,costheta];
pts = pts*R;

% translate
pts(:,1) = pts(:,1) + x;
pts(:,2) = pts(:,2) + y;

% plot
set(h,'xdata',pts([1:3,1],1),'ydata',pts([1:3,1],2));

end


%SerialLink.GRAVJAC Fast gravity load and Jacobian
%
% [TAU,JAC0] = R.gravjac(Q) is the generalised joint force/torques due to
% gravity (1xN) and the manipulator Jacobian in the base frame (6xN) for
% robot pose Q (1xN), where N is the number of robot joints.
%
% [TAU,JAC0] = R.gravjac(Q,GRAV) as above but gravity is given explicitly
% by GRAV (3x1).
%
% Trajectory operation::
%
% If Q is MxN where N is the number of robot joints then a trajectory is
% assumed where each row of Q corresponds to a pose.  TAU (MxN) is the
% generalised joint torque, each row corresponding to an input pose, and
% JAC0 (6xNxM) where each plane is a Jacobian corresponding to an input pose.
%
% Notes::
% - The gravity vector is defined by the SerialLink property if not explicitly given.
% - Does not use inverse dynamics function RNE.
% - Faster than computing gravity and Jacobian separately.
%
% Author::
% Bryan Moutrie
%
% See also SerialLink.pay, SerialLink, SerialLink.gravload, SerialLink.jacob0.

% Copyright (C) Bryan Moutrie, 2013-2015
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%

function [tauB, J] = gravjac(robot, q, grav)

    n = robot.n;
    revolute = [robot.links(:).isrevolute];
    if ~robot.mdh
        baseAxis = robot.base(1:3,3);
        baseOrigin = robot.base(1:3,4);
    end

    poses = size(q, 1);
    tauB = zeros(poses, n);
    if nargout == 2, J = zeros(6, robot.n, poses); end

    % Forces
    force = zeros(3,n);
    if nargin < 3, grav = robot.gravity; end
    for joint = 1: n
        force(:,joint) = robot.links(joint).m * grav;
    end

    % Centre of masses (local frames)
    r = zeros(4,n);
    for joint = 1: n
        r(:,joint) = [robot.links(joint).r'; 1];
    end
    com_arr = zeros(3, n);

    for pose = 1: poses

        [Te, T] = robot.fkine(q(pose,:));

        jointOrigins = squeeze(T(1:3,4,:));
        jointAxes = squeeze(T(1:3,3,:));

        if ~robot.mdh
            jointOrigins = [baseOrigin, jointOrigins(:,1:end-1)];
            jointAxes = [baseAxis, jointAxes(:,1:end-1)];
        end

        % Backwards recursion
        for joint = n: -1: 1

            com = T(:,:,joint) * r(:,joint); % C.o.M. in world frame, homog
            com_arr(:,joint) = com(1:3); % Add it to the distal others

            t = 0;
            for link = joint: n % for all links distal to it
                if revolute(joint)
                    d = com_arr(:,link) - jointOrigins(:,joint);
                    t = t + cross3(d, force(:,link));
                    % Though r x F would give the applied torque and not the
                    % reaction torque, the gravity vector is nominally in the
                    % positive z direction, not negative, hence the force is
                    % the reaction force
                else
                    t = t + force(:,link); %force on prismatic joint
                end
            end

            tauB(pose,joint) = t' * jointAxes(:,joint);
        end

        if nargout == 2
            J(:,:,pose) = makeJ(jointOrigins,jointAxes,Te(1:3,4),revolute);
        end

    end

end

function J = makeJ(O,A,e,r)
    J(4:6,:) = A;
    for j = 1:length(r)
        if r(j)
            J(1:3,j) = cross3(A(:,j),e-O(:,j));
        else
            J(:,j) = J([4 5 6 1 2 3],j); %J(1:3,:) = 0;
        end
    end
end

function c = cross3(a,b)
    c(3,1) = a(1)*b(2) - a(2)*b(1);
    c(1,1) = a(2)*b(3) - a(3)*b(2);
    c(2,1) = a(3)*b(1) - a(1)*b(3);
end


function Population = EnvironmentalSelection(Population,N,objective_set)
% The environmental selection of SIBEA-kEMOSS

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Liangli Zhen

    PopObj = Population.objs;

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(PopObj(:,objective_set),N);
    Next = FrontNo < MaxFNo;

    %% Select the solutions in the last front based on their HV loss
    Last = find(FrontNo==MaxFNo);
    % Calculate the WHV loss of each solution in the last front
    HVLoss   = CalHVLoss(PopObj(Last,objective_set),FrontNo(Last));
    [~,Rank] = sort(HVLoss,'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
end


%% Testing ground for quad mesh adaptive refinement
% This is the testing utilizing VEM's polytopal structure
% without using the tree-structure to maintain the mesh data.
%
% Outer loop: # of vertices
% inner loop:
%
% Step 1: find the center/centroid P of marked elements
% Remark: current version using naive way of locating centroid
%
% Step 2: quad-sect using P, find duplicate nodes
% Step 3: form new elements replacing the marked elements using cell
% Step 4: add vertices to the neighbor elements through edges
%
% Two sets of mesh are kept:
% 1. For refinement elem: the first 4 vertices are always the square clockwise,
% after those are the extra vertices for polygon.
% 2. For computation elemRefine: re-ordered vertices mesh.

clear; close all;

%%
[node,elemOrig] = squarequadmesh([0,1,0,1],0.25);
figure(1);
showmesh(node,elemOrig);
findnode(node);
set(gcf,'color','w','Position', [100 600 500 500])

% local ordering should always be kept as
% 2 -- 3
% |    |
% 1 -- 4

T = auxstructurequad(elemOrig);

elem = num2cell(elemOrig,2); % convert elem matrix to cell

elemVertexNumber = cellfun('length',elem);

N = size(node,1);

%%
% Nv = min(elemVertexNumber):max(elemVertexNumber)
Nv=4;

% find polygons with Nv vertices
idx = find(elemVertexNumber == Nv); % index of elements with Nv vertices
NT = length(idx); % the number of elements having the same number of vertices
% vertex index and coordinates
vertex = cell2mat(elem(idx));

x = reshape(node(vertex,1),NT,Nv);
y = reshape(node(vertex,2),NT,Nv);

xmax = max(x,[],2); % the rightmost vertices' x-coords for each element
ymax = max(y,[],2); % the uppermost vertices' y-coords for each element
xmin = min(x,[],2); % the leftmost vertices' x-coords for each element
ymin = min(y,[],2); % the lowermost vertices' y-coords for each element

xc = 0.5*(xmax + xmin);
yc = 0.5*(ymax + ymin);

x_nb = circshift(x,[0,-1]);
y_nb = circshift(y,[0,-1]);

%% marking elements
idxMarkedElem = idx(randi(NT, [floor(NT/3), 1]));
% idxMarkedElem = idx([1,3]);
NmarkedElem = length(idxMarkedElem);

NnodeNew = 5*NmarkedElem;
% number of the new nodes per Nv consisting duplicated nodes

% o -- 2 -- o
% |    |    |
% 1 -- 5 -- 3
% |    |    |
% o -- 4 -- o
% o: original vertices
% 1 to 5: local indices of newly added vertices
% the order is consistent with neighbor ordering

nodeNew(1:5:NnodeNew,:) = [xmin(idxMarkedElem,1), yc(idxMarkedElem,1)]; % left
nodeNew(2:5:NnodeNew,:) = [xc(idxMarkedElem,1), ymax(idxMarkedElem,1)]; % top
nodeNew(3:5:NnodeNew,:) = [xmax(idxMarkedElem,1), yc(idxMarkedElem,1)]; % right
nodeNew(4:5:NnodeNew,:) = [xc(idxMarkedElem,1), ymin(idxMarkedElem,1)]; % bottom
nodeNew(5:5:NnodeNew,:) = [xc(idxMarkedElem,1), yc(idxMarkedElem,1)]; % center

[nodeNewU, idxN, idxU] = unique(nodeNew,'rows','legacy');
% nodeNewU = nodeNew(idxN,:) and nodeNew = nodeNewU(idxU,:)

idxNodeNew = N+1:N+NnodeNew; % unique indices of new nodes

%% generate new elements on marked elements
elem2nodeNew = reshape(idxNodeNew(idxU), [5, NmarkedElem])';
% elem2nodeNew(i,:): i-th marked elem's newly added vertices
% elem2nodeNew(:,1): center of the marked elements
% elem2nodeNew(:,2:5): counterclockwisely ordered new edge vertices from
% the left edge locally

node = [node; nodeNewU];

elemMarked = cell2mat(elem(idxMarkedElem));

% localNewVertex = [2 1 3; 5 1 2; 4 1 5; 3 1 4]; %not very helpful

elemNew1 = horzcat(elemMarked(:,1), elem2nodeNew(:,[1 5 4]));
elemNew2 = horzcat(elem2nodeNew(:,1), elemMarked(:,2), elem2nodeNew(:,[2 5]));
elemNew3 = horzcat(elem2nodeNew(:,[5 2]), elemMarked(:,3), elem2nodeNew(:,3));
elemNew4 = horzcat(elem2nodeNew(:,[4 5 3]), elemMarked(:,4));

NelemNew = 4*NmarkedElem;

%% change elements' vertices on unrefined neighbor elements
% no need to check whether a vertex is added twice b/c
% the marked elements will get over-written in the next routine

elemMarkedNB = T.neighbor(idxMarkedElem,:);
% idxNb = ~(elemMarkedNB==idxMarkedElem);
elemMarkedNB = num2cell(elemMarkedNB,2);

for i = 1:size(elemMarked,1)
    for j = 1:size(elemMarkedNB{i},2)
        elem{elemMarkedNB{i}(:,j)} = ...
         [elem{elemMarkedNB{i}(:,j)}, elem2nodeNew(i,j)];
    end
end

%% vertically concat all new elements into a cell
elemNew = reshape([elemNew1'; elemNew2'; elemNew3'; elemNew4'], [4, NelemNew])';

elem(idxMarkedElem) = mat2cell(elemNew, repmat(4,[1,NmarkedElem]));

% elemTemp = vertcat(elem{:});
% above doesn't work if elem is of different size when nested

outTemp = cellfun(@(x) num2cell(cat(2,x),2),elem,'UniformOutput',false);
elemRefine = cellflat(outTemp)';

% celldisp(elemRefine)

%% reorder the polygon with vertices > 4
% to-do: rewrite the for loop to cellfun
elemVertexNumberRefine = cellfun('length',elemRefine);
idxReorder = find(elemVertexNumberRefine > 4);

for i = 1:length(idxReorder)
    % compute the center (simple mean suffices) and sorting atan2
    elemTemp = elemRefine{idxReorder(i)};
    center = mean(node(elemTemp,:),1);
    vert2center = node(elemTemp,:) - center;
    [~, idxOrdered] = sort(atan2(vert2center(:,1), vert2center(:,2)));
    elemRefine{idxReorder(i)} = elemTemp(idxOrdered);
end

Nrefine = size(node,1); % update number of nodes

%%
figure(2);
showmeshpoly(node,elemRefine);
plot(xc,yc,'o');
findnode(node);
set(gcf,'color','w','Position', [650 600 500 500])


% pdfbcalc_imagekld.m
% written by: Duncan Po
% Date: November 16, 2002
% calculate the Kublick-Liebler distance between two images based on
% on their hidden markov tree parameters
% Usage:    kld = pdfbcalc_imagekld(savename, sdir, model1, model2)
%           kld = pdfbcalc_imagekld(savename, sdir, image1, model2, imformat1)
%           kld = pdfbcalc_imagekld(savename, sdir, image1, image2, imformat1,
%                       imformat2)
% Inputs:   model1      - first Hidden Markov Tree Model
%           model2      - second Hidden Markov Tree Model
%           image1      - first image file name
%           image2      - second image file name
%           imformat1   - format of the first image file
%           imformat2   - format of the second image file
%           savename    - filename to save the model of image1
%                       - set to '' if no save desired
%           sdir        - full pathname to save the model 1 (also set to ''
%                           if savename == '')
% Output:   kld         - the Kullback Liebler distance between the trees
%           kld2        - KLD estimation using Monte Carlo method

%function [kld, kld2] = pdfbcalc_imagekld(savename, sdir, model1, model2, ...
%    imf1, imf2)
function [kld, kld2] = pdfbcalc_imagekld(savename, sdir, model1, model2, ...
    imf1, imf2)

% initialize the kld
kld = 0;
kld2 = 0;

if nargin == 6
    % this case is when the images are provided but not the models
    image1 = model1;
    image2 = model2;

    % train the HMT models first
    [model1, dummy] = pdfbtrainimagethmt(image1, imf1, '', 0.01);
    [model2, dummy] = pdfbtrainimagethmt(image2, imf2, '', 0.01);

elseif nargin == 5
    % this case is when we have one model and one image, need to
    % train a second model using the image provided
    image1 = model1;

    % train the HMT model
    [model1, dummy] = pdfbtrainimagethmt(image1, imf1, '', 0.01);

    %there is also the case for two models and no image when nargin==3
    %in that case, no training is needed
end;

if strcmp(savename, '') ~= 1
    file = sprintf('%s%s', sdir, savename);
    save(file,'model1');
end;

N = 60;
nlevel = model1{1}.nlevels;

for k = 1:nlevel
    levndir(k) = log2(length(model1{1}.stdv{k}));
end;

% treat each tree as independent and using chain rule of KLD for
% independent variables, simply add up the distances
for index = 1:length(model1)
    minkld = 10000;
    dbmodels = pdfbcreate_equiv_models(model1{index});
    for mmm = 1:length(dbmodels)
        tkld = pdfbcalc_KLD(nlevel, levndir, model2{index}, dbmodels{mmm});
        if tkld < minkld
            minkld = tkld;
        end;
    end;
    kld = kld + minkld;
end;

% Also apply the Monte Carlo method to verify the KLD estimation
for index = 1:length(model1)
    kld2 = kld2 + pdfbest_KLD(nlevel, levndir ,model2{index}, ...
        model1{index}, N);
end;


filename='RVE_Square_Triangle';
ptype = 'MICRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'circleInclusion';
cost={'chomog_alphabeta','perimeter'};
weights=[1 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.5;
Perimeter_target=1;
optimality_final =1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;

%Micro
epsilon_isotropy_initial=1e-1;
epsilon_isotropy_final = 1e-3;
micro.alpha =[1 0.5 0]';
micro.beta =[1 0.5 0]';


function cpp = subsetconnectivity ( t, p, nds )

%*****************************************************************************80
%
%% SUBSETCONNECTIVITY: connectivity of boundary node subset in a triangulation.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    29 April 2014
%
%  Author:
%
%    Marcus Garvie
%
%  Parameters:
%
%    Input, real P(NP,2), the coordinates of a set of nodes.
%
%    Input, integer T(NT,3), a list of the nodes which make up each
%    triangle of a triangulation of the nodes in P.
%
%    Input, integer NDS(NN,1), a list of nodes comprising a subset of the
%    boundary nodes.
%
%    Output, CPP(NE,2), a list of edges, each edge defined by 2 nodes from
%    the list NDS.
%
  cpp = [];
%
% Work out connectivity for the whole boundary.
%
  edges = boundedges (p,t);
%
%  NO_EDGES = Number of edges in whole boundary.
%
  [no_edges,~] = size(edges);
%
%  NN = Number of nodes in the subset of boundary nodes.
%
  NN = length(nds);
%
%  Find the edges in the subset of the boundary NDS.
%
  count = 0;

  for i = 1:no_edges

    node1 = edges(i,1);
    node2 = edges(i,2);
    bin1 = ismember(node1,nds);
    bin2 = ismember(node2,nds);

    if (bin1==1) & (bin2==1)
      count = count + 1;
      cpp(count,:) = edges(i,:);
    end

  end

  return
end


function res = ne(p,q)
%NE           Implements  p ~= q  for polynomials
%
%Result 1 iff p and q are mathematically not identical
%

% written  08/28/00     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  p = removevars(permvars(polynom(p),'lex'));
  q = removevars(permvars(polynom(q),'lex'));
  res = ~( isequal(p.e,q.e) & isequal(p.c,q.c) & isequal(p.v,q.v) );


function value = i4_bset ( i4, pos )

%*****************************************************************************80
%
%% I4_BSET returns a copy of an I4 in which the POS-th bit is set to 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Military Standard 1753,
%    FORTRAN, DoD Supplement To American National Standard X3.9-1978,
%    9 November 1978.
%
%  Parameters:
%
%    Input, integer I4, the integer to be tested.
%
%    Input, integer POS, the bit position, between 0 and 31.
%
%    Output, integer VALUE, a copy of I4, but with the POS-th bit
%    set to 1.
%
  i4_huge = 2147483647;

  value = round ( i4 );

  if ( pos < 0 )

  elseif ( pos < 31 )

    add = 1;

    if ( 0 <= i4 )
      j = i4;
    else
      j = ( i4_huge + i4 ) + 1;
    end

    for k = 1 : pos
      j = floor ( j / 2 );
      add = add * 2;
    end

    if ( mod ( j, 2 ) == 0 )
      value = i4 + add;
    end

  elseif ( pos == 31 )

    if ( 0 < i4 )
      value = - ( i4_huge - i4 ) - 1;
    end

  elseif ( 31 < pos )

    value = i4;

  end

  return
end


function scales = svargplvmShowScales(model, printPlots, modalities, dims, thresh, normaliseScales, customLeg, logScales)

% SVARGPLVMSHOWSCALES Show the scales of a svargplvmModel graphically
%
% SEEALSO : svargplvmFindSharedDims
%
% COPYRIGHT: Andreas C. Damianou, 2012, 2013
%
% VARGPLVM

if nargin < 2 || isempty(printPlots)
    printPlots = 2;
end

if nargin < 3 || isempty(modalities)
    modalities = 1:length(model.comp);
else
    printPlots = true;
end

if nargin < 4 || isempty(dims)
    dims = 1:model.q;
end

if nargin < 5 || isempty(thresh)
    thresh = -Inf;
end

if nargin < 6 || isempty(normaliseScales)
    normaliseScales = false;
end

if nargin < 7
    customLeg = {}; % Custom legend
end

if nargin < 8 || isempty(logScales)
    logScales = false;
end

totalModels = length(modalities);

if printPlots
    for i=1:length(model.comp)
        if strcmp(model.comp{i}.kern.type, 'rbfardjit')
            scales{i} = model.comp{i}.kern.inputScales(dims);
        else
            scales{i} = model.comp{i}.kern.comp{1}.inputScales(dims);
        end
    end
    if logScales
        origScales = scales;
        for k=1:totalModels
            scales{modalities(k)} = scales{modalities(k)}./max(scales{modalities(k)});
            scales{modalities(k)} = log(scales{modalities(k)});%logN(scales{modalities(k)},1.01);
            a=0; b=1; % Scale data between a and b
            X=scales{modalities(k)};
            X = (X-min(min(X))).*(b-a)./(max(max(X))-min(min(X))) + a;
            scales{modalities(k)}=X;
        end
    end
    if (totalModels == 2 || normaliseScales) && ~logScales
        origScales = scales;
        for k = 1:totalModels
            scales{modalities(k)}=scales{modalities(k)}./max(scales{modalities(k)});
        end
    end
    for k=1:totalModels
       scales{modalities(k)}(scales{modalities(k)}<thresh) = 0;
    end
    if totalModels == 2
        maxScales1 = max(scales{modalities(1)});
        maxScales2 = max(scales{modalities(2)});
        %scales{modalities(1)} = scales{modalities(1)}./maxScales1;
        %scales{modalities(2)} = scales{modalities(2)}./maxScales2;
        x=1:size(scales{modalities(1)},2);
        K=0.5;
        bar1=bar(x, scales{modalities(1)}, 'FaceColor', 'b', 'EdgeColor', 'b');
        set(bar1,'BarWidth',K);
        hold on;
        bar2=bar(x, scales{modalities(2)}, 'FaceColor', 'r', 'EdgeColor', 'r');
        set(bar2,'BarWidth',K/2);
        hold off;
        if isempty(customLeg)
            legend(['scales1 (x ' num2str(maxScales1) ')'],['scales2 (x ' num2str(maxScales2) ')'])
        else
            legend(customLeg);
        end
        scales = origScales;
    else
        % Too many models. Plot one at a time
        %if totalModels > 10 && exist('hsvargplvmShowScales','file')
             %mm.layer{1} = model;
             %mm.H=1;
             %mm.layer{1}.M = totalModels;
             %hsvargplvmShowScales(mm);
        %else
            for i=1:totalModels
                if totalModels > 10
                    pause
                    %figure;
                elseif totalModels > 1
                    subplot(1,totalModels, i) ;
                end
                bar(scales{modalities(i)}); title(['Modality ' num2str(modalities(i))])
            end
        %end
    end
else
    for i=1:length(model.comp)
        if strcmp(model.comp{i}.kern.type, 'rbfardjit')
            fprintf('# Scales of model %d: ', i)
            fprintf('%.4f  ',model.comp{i}.kern.inputScales(dims));
            fprintf('\n');
            scales{i} = model.comp{i}.kern.inputScales(dims);
        else
            fprintf('# Scales of model %d: ', i)
            fprintf('%.4f  ',model.comp{i}.kern.comp{1}.inputScales(dims));
            fprintf('\n');
            scales{i} = model.comp{i}.kern.comp{1}.inputScales(dims);
        end
        scales{i}(scales{i}<thresh) = 0;
    end
end

function y=logN(x,N)
y = log(x)/log(N);