text:
function [p] = eeg_contours_engine(p)

% eeg_contours_engine - Generates contour/topographic maps
%
% Usage: [p] = eeg_contours_engine(p)
%
% p is the eeg_toolbox struct (see eeg_toolbox_defaults)
%
% Given:
%   data file of electrode positions (see elec_load for help)
%   data file of electrode voltage values (see eeg_ascii_load for help)
%   other parameters for selection and plot control
%
%   Parameters can be passed as a structure (p) with the function call or
%   passed in the file 'eeg_contours_default.txt'.  A standard set of
%   parameters can be created using 'eeg_contours_create_defaults'. See
%   that function for more information about the parameters created.
%
% Output:
%	various topographic maps and 2D or 3D contour plots
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:51 $

% Licence:  Gnu GPL, no express or implied warranties
% History:  08/99 Chris Harvey,	Created, combining work by Darren Weber, Murk Bottomer
%           10/99 CRH	Accepts input parameters, provides some validation
%           07/01 Darren.Weber_at_radiology.ucsf.edu
%               - modified parameter handling, now using 'p' structure
%               - now calls separate functions to create/read/write defaults
%               - modified most aspects of code to adapt to development of other
%                 functions called from this function
% Develop:  08/01 Darren.Weber_at_radiology.ucsf.edu
%               - Needs scripting facility to wind through time points in
%                 a range of timepoints, outputing contour plot(s) for each in
%                 a given image format (EPS, GIF, TIF, etc.) or a movie of
%                 the timeseries.
%           05/02 Darren.Weber_at_radiology.ucsf.edu
%               - now integrated with mesh interpolation and calls
%                 animation interface, with image saving options
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('p','var'),
  fprintf('Setting default parameters.\n');
 [p] = eeg_toolbox_defaults('create');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% load electrode co-ordinates
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(p.elec.data),[p] = elec_open(p); end

%elecLabels = p.elec.data.label;
X = p.elec.data.x;
Y = p.elec.data.y;
Z = p.elec.data.z;

Xrad = p.elec.data.R(1);
Yrad = p.elec.data.R(2);
Zrad = p.elec.data.R(3);

elecNumElectrodes = length(p.elec.data.x);

% Estimate sphere or ellipse that best fits electrode co-ordinates
switch p.elec.shape
  case 'ellipse'
    Xp = p.elec.data.Xel;
    Yp = p.elec.data.Yel;
    Zp = p.elec.data.Zel;
  case 'sphere'
    Xp = p.elec.data.Xsp;
    Yp = p.elec.data.Ysp;
    Zp = p.elec.data.Zsp;
  otherwise
    Xp = p.elec.data.x;
    Yp = p.elec.data.y;
    Zp = p.elec.data.z;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Read voltage data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(p.volt.data),[p] = eeg_open(p); end

[voltNumTimePoints, voltNumElectrodes] = size(p.volt.data);

% Validate voltage matrix orientation against electrode file

if (voltNumElectrodes ~= elecNumElectrodes)
  fprintf('\nWarning: # electrodes, data file %d, electrode file %d\n', voltNumElectrodes, elecNumElectrodes);
  if (voltNumTimePoints == elecNumElectrodes)
    % Assume data file needs rotation
    fprintf('Continuing with data file rotated\n');
    p.volt.data = p.volt.data';   [voltNumTimePoints, voltNumElectrodes] = size(p.volt.data);
  else
    fprintf('\nError: Cannot reconcile electrodes with voltage data.\n');
    return
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Select a timePoint
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if or(isequal(p.clickTimePoint, 1),isempty(p.volt.samplePoint))
  figure('NumberTitle','off','Name','MouseClick the TimePoint')
  plot(p.volt.data);
  [Xt,Yt] = ginput(1);
  p.volt.samplePoint = round(Xt);
  close
end
if (p.volt.samplePoint > voltNumTimePoints)
  msg = sprintf(' Selected timepoint exceeds data points of %d\n\n', voltNumTimePoints);
  warning(msg);
  p.volt.samplePoint = voltNumTimePoints;
end
if ~isempty(p.volt.timeArray),
  p.volt.sampleTime = p.volt.timeArray(p.volt.samplePoint);
end

V = p.volt.data(p.volt.samplePoint,:)';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Determine/validate the min, max range and the step size
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

switch p.rangeMethod
  case 'minmaxall', % Min/Max,all points
    p.maximumIntensity = max(max(p.volt.data));
    p.minimumIntensity = min(min(p.volt.data));
  case 'minmaxone', % Min/Max, single point
    p.maximumIntensity = max(max(V));
    p.minimumIntensity = min(min(V));
  case 'minmaxabs', % Min/Max, Absolute
    absmax = max(max(abs(V)));
    p.maximumIntensity =  absmax;
    p.minimumIntensity = -absmax;
  otherwise
    % check that specified intensity range is defined
    if isempty(p.maximumIntensity),
      p.maximumIntensity = max(max(V)); end
    if isempty(p.minimumIntensity),
      p.minimumIntensity = min(min(V)); end
end

switch p.contour.stepMethod
  case 0  % StepSize method
    p.contour.levels = eeg_contour_levels( p.contour.stepSize, V );
    p.contour.Nsteps = length(p.contour.levels);
  otherwise % Number of steps method
    p.contour.stepSize = abs(p.maximumIntensity - p.minimumIntensity) / p.contour.Nsteps;
    p.contour.levels = eeg_contour_levels( p.contour.stepSize, V );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Draw the plots
%	X,Y,Z		    = actual electrode placements
% 	Xel,Yel,Zel 	= electrode position on the ellipsoid surface
%	gridSize	    = number of grid lines to use on each plane
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Interpolate surface
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Determine interpolation gridding
m = max(abs([Xp;Yp]));
m = ceil(m);
if (p.grid.method == 2), xg = -m:p.grid.res:m;             % grid resolution method
else,                   xg = linspace(-m,m,p.grid.size);  % grid size method
end
yg = xg';
[Xi,Yi,Zi] = griddata(Xp,Yp,Zp,xg,yg,p.interpMethod);    % Surface interpolation
Vi         = griddata(Xp,Yp,V ,xg,yg,p.interpMethod);    % Voltage interpolation

% Calculate voltage at refined electrode vertices
%if ~isempty(p.elec.data.Vsp),
%    p.elec.data.Psp = p.elec.data.Isp * V;
%end

% Calculate voltage at scalp mesh vertices
if p.mesh.plotSurf,

  % get the scalp mesh
  [p.mesh.current,meshExists] = mesh_check(p,'scalp');

  if isempty(p.mesh.current),
    fprintf('...no scalp mesh, using electrode surface.\n');
    p.mesh.plotSurf = 0;
    p.elec.plotSurf = 1;
  else
    % Assume that p.mesh.data has this field, as it should
    % be initialised by mesh_open, but check anyway
    if ~isfield(p.mesh.data,'Cdata'),
     [p] = mesh_scalp_interp(p);
    end
    % Confirm that p.mesh.data.timeseries{p.mesh.current} has data
    if length(p.mesh.data.Cdata) < p.mesh.current,
     [p] = mesh_scalp_interp(p);
    end
    if isempty(p.mesh.data.Cdata{p.mesh.current}),
     [p] = mesh_scalp_interp(p);
    end

    % Now we should have a mesh timeseries

    % Verify the timeseries data against the elec timeseries;
    % if generated with mesh_scalp_interp, they should be equal
    TMP = p.mesh.data.Cdata{p.mesh.current};
    Nelec = size(p.volt.data,2);
    if ~isequal(p.volt.data',TMP(1:Nelec,:)),
      % Something has changed, either a new electrode
      % set or scalp mesh, so run the interpolation
      msg = sprintf('scalp time series not consistent with electrode time series - recalculating.\n');
      warning(msg);
      p.mesh.data.Cdata{p.mesh.current} = [];
     [p] = mesh_scalp_interp(p);
      TMP = p.mesh.data.Cdata{p.mesh.current};
      if ~isequal(p.volt.data',TMP(1:Nelec,:)),
        msg = sprintf('scalp timeseries not consistent with electrode timeseries.\n');
        error(msg);
      end
    end
    clear TMP;
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% surface plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if or(p.elec.plotSurf,p.mesh.plotSurf),

  if ~isequal(p.volt.sampleTime,0),
    name = sprintf('Surface: %s @ %8.2f msec',p.volt.file, p.volt.sampleTime);
  else
    name = sprintf('Surface: %s',p.volt.file);
  end

  if p.elec.plotSurf,

    [p.mesh.current,meshExists] = mesh_check(p,'elec');

    if ~meshExists,
      p.mesh.data.meshtype{p.mesh.current}    = 'elec';
      p.mesh.data.vertices{p.mesh.current}    = [Xp,Yp,Zp];
      p.mesh.data.faces{p.mesh.current}       = convhulln([Xp,Yp,Zp]);
      p.mesh.data.lapmat{p.mesh.current}      = [];
      p.mesh.data.lapint{p.mesh.current}      = [];
      p.mesh.data.timeseries{p.mesh.current}  = p.volt.timeArray(:,1);
      p.mesh.data.Cdata{p.mesh.current}       = p.volt.data';
    end

    %p = eeg_plot_surf(p);
    %return

    H.gui = figure('NumberTitle','off','Name',name,...
      'PaperType','A4','PaperUnits','centimeters');

    set(gca,'Projection','perspective')
    set(gca,'DataAspectRatio',[1 1 1]);

    p.elec.patch = patch('vertices',[Xp,Yp,Zp],'faces',convhulln([Xp,Yp,Zp]),...
      'facevertexCdata',V,'facecolor','interp','EdgeColor','none',...
      'FaceLighting','phong');
    %patch('vertices',p.elec.data.Vsp,'faces',p.elec.data.Fsp,...
    %      'facevertexCdata',p.elec.data.Psp,'facecolor','interp','EdgeColor','none');

    set(gca,'Projection','perspective')
    set(gca,'DataAspectRatio',[1 1 1]);
    axis off tight vis3d

    lighting phong
    set(H.gui,'Renderer','zbuffer')

    % Create a light above the z axis
    H.light = light('style','infinite','position',[0 0 1000]);

    % For a near skin color
    %set(gca,'AmbientLightColor',[.9 .8 .7]);

    % MATERIAL([ka kd ks n sc]) sets the ambient/diffuse/specular strength,
    %    specular exponent and specular color reflectance of the objects.
    %p.reflect = material('dull');
    p.reflect{1} = 0.9;
    p.reflect{2} = 0.1;
    p.reflect{3} = 0.0;
    p.reflect{4} = 500;
    p.reflect{5} = 0;
    set(p.elec.patch,'AmbientStrength',p.reflect{1});
    set(p.elec.patch,'DiffuseStrength',p.reflect{2});
    set(p.elec.patch,'SpecularStrength',p.reflect{3});
    set(p.elec.patch,'SpecularExponent',p.reflect{4});
    set(p.elec.patch,'SpecularColorReflectance',p.reflect{5});

    if p.elec.plot,
      hold on, plot3(Xp,Yp,Zp,'k.');
    end

    set(gca,'Visible','off');
    colormap(p.colorMap.map);
    caxis([p.minimumIntensity p.maximumIntensity]);
    colorbar

    H.p = p;
    H.p.mesh.plotSurf = 0;
    set(H.gui,'userdata',H);

    SaveGraphics(H.gui,'_3Delec_',p);
    if isequal(exist('mouse_rotate'),2),
      mouse_rotate;
    else
      rotate3D;
    end
    if isequal(exist('gui_topo_animate'),2),
      gui_topo_animate('init',p);
    end
  end

  % Plot a mesh surface

  if p.mesh.plotSurf,

    [p.mesh.current,meshExists] = mesh_check(p,'scalp');

   [p] = eeg_plot_surf(p);

    return
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Contour in 2-D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%CONTOUR(Z,N) and CONTOUR(X,Y,Z,N) draw N contour lines,
%overriding the automatic value.
%CONTOUR(Z,V) and CONTOUR(X,Y,Z,V) draw LENGTH(V) contour lines
%at the values specified in vector V.

if isequal(p.contour.raw2D,1),

  if ~isequal(p.volt.sampleTime,0),
    name = sprintf('2-D Contour: %s @ %8.2f msec',p.volt.file, p.volt.sampleTime);
  else
    name = sprintf('2-D Contour: %s',p.volt.file);
  end
  fig = figure('NumberTitle','off','Name',name); colormap(p.colorMap.map);
  set(gca,'Projection','perspective')
  %set(gca,'Projection','orthographic')
  set(gca,'DataAspectRatio',[1 1 1]);

  %trisurf(delaunay(Xp,Yp),Xp,Yp,Zp,V,'EdgeColor','none','FaceColor','interp');
  %hold on, plot3(Xp,Yp,Zp,'.');
  %contour(Xi,Yi,Vi, p.contour.Nsteps) %, colorbar, hold on

  [c,ch,cf] = contourf(Xi,Yi,Vi,p.contour.levels);
  absmax = max(max(abs(Vi)));caxis([-absmax absmax]);
  %[C,CH,CF] = CONTOURF(...) also returns a column vector CH of handles
  %to PATCH objects and the contour matrix CF for the filled areas. The
  %UserData property of each object contains the height value for each
  %contour.

  % FaceColor is the contour patch color
  % EdgeColor is the contour line color
  for i=1:size(ch),
    height = get(ch(i),'UserData');

    if     (height > 0),
      set(ch(i),'LineStyle', '-'); % options are: -, --, :, -., none
      set(ch(i),'EdgeColor',[1 1 1]) % white contour lines, black = [0 0 0] (ie, [R G B])
    elseif (height < 0),
      set(ch(i),'LineStyle', ':'); % options are: -, --, :, -., none
    else
      set(ch(i),'LineStyle', '-'); % options are: -, --, :, -., none
      set(ch(i),'LineWidth',2);
    end
  end

  set(gca,'Visible','off'); %hold on; DrawHead(Xrad,'black'); hold off;
  colorbar

  SaveGraphics(fig,'_2D_',p);
  if isequal(exist('mouse_rotate'),2),
    mouse_rotate;
  else
    rotate3D;
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Projected Contour in 2-D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isequal(p.contour.plot2D,1)

  % Project 3D electrode locations onto 2D plane.
  [X2p,Y2p] = elec_3d_2d(Xp,Yp,Zp,Zrad);
  m = max(abs([X2p;Y2p]));
  m = ceil(m);
  if (p.grid.method == 2)
    % Use grid resolution method
    X2g = -m:p.grid.res:m;
  else
    % Use grid size method
    X2g = linspace( -m, m, p.grid.size );
  end
  Y2g = X2g';
  [X2i Y2i V2i] = griddata(X2p,Y2p,V,X2g,Y2g,p.interpMethod);

  if ~isequal(p.volt.sampleTime,0),
    name = sprintf('Projected 2D Contours: %s @ %8.2f msec',p.volt.file, p.volt.sampleTime);
  else
    name = sprintf('Projected 2D Contours: %s',p.volt.file);
  end
  fig = figure('NumberTitle','off','Name',name);
  set(gca,'Projection','perspective')
  %set(gca,'Projection','orthographic')
  set(gca,'DataAspectRatio',[1 1 1]);

  colormap(p.colorMap.map);
  contourf(X2i,Y2i,V2i, p.contour.Nsteps);
  absmax = max(max(abs(V2i)));caxis([-absmax absmax]);colorbar
  xlabel('X'); ylabel('Y');

  SaveGraphics(fig,'_proj2D_',p);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Projected Contour in 3-D
% Draw the contour lines,
%   Get the contours in 2-D.
%   For each point in each contour line, know x,y
%   Calculate z for the x,y from ellipse formula
%   Plot3 point x,y,z
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (p.contour.plot3D == 1)

  % Project 3-D electrode locations onto 2-D plane.
  [X2p,Y2p] = elec_3d_2d(Xp,Yp,Zp,Zrad);
  m = max(abs([X2p;Y2p]));
  m = ceil(m);
  if isequal(p.grid.method,2)
    % Use grid resolution method
    X2g = -m:p.grid.res:m;
  else
    % Use grid size method
    X2g = linspace( -m, m, p.grid.size );
  end
  Y2g = X2g';

  [X2i Y2i V3i] = griddata(Xp, Yp, V,X2g,Y2g,p.interpMethod);

  % Caculate the contour matrix
  C = contourc(X2g, Y2g, V3i, p.contour.Nsteps);

  if ~isequal(p.volt.sampleTime,0),
    name = sprintf('3D Contours: %s @ %8.2f msec',p.volt.file, p.volt.sampleTime);
  else
    name = sprintf('3D Contours: %s',p.volt.file);
  end

  %fig = figure('NumberTitle','off','Name',name);
  %set(gca,'Projection','perspective')
  %set(gca,'DataAspectRatio',[1 1 1]);
  %[C,Cpatch] = contourf(X2g, Y2g, V3i, p.contour.Nsteps);
  %get(Cpatch(1))
  %P.cdata    = get(Cpatch,'CData');
  %P.vertices = get(Cpatch,'Vertices');
  %P.faces    = get(Cpatch,'Faces');
  %close(fig);
  %fig = figure('NumberTitle','off','Name',name); hold on
  %i = 0;
  %while i < (length(Cpatch) - 1),
  %%vertices = P.vertices{i};
  %i = i + 1;
  %vertices = [P.vertices{i}; P.vertices{i+1}];
  %Xc = vertices(:,1);
  %Yc = vertices(:,2);
  %[X3,Y3,Z3] = elec_2d_3d(Xc,Yc,Xrad,Yrad,Zrad);
  %tri = delaunay(X3,Y3);
  %Htri = trisurf(tri,X3,Y3,Z3,P.cdata{i},'EdgeColor','none','FaceColor','flat');

  %%vertices = [X3 Y3 Z3];
  %%Hpatch = patch('Vertices',vertices,'Faces',P.faces{i},'CData',P.cdata{i},'FaceColor','flat');
  %contourValues(i) = P.cdata{i};
  %end

  %view(0,90), axis tight, rotate3d
  %set(gca,'Visible','off');
  %colormap(p.colorMap.map);
  %absmax = max(abs(contourValues));
  %caxis([-absmax absmax]);colorbar;

  fig = figure('NumberTitle','off','Name',name); hold on
  set(gca,'Projection','perspective')
  %set(gca,'Projection','orthographic')
  set(gca,'DataAspectRatio',[1 1 1]);

  % When we view the contour lines, those on the other side of
  % the head can be seen, which makes it confusing.

  % The array C is 2-row, x on 1st, y on 2nd.
  % Each contour set has a preceeding column, where
  % 1st is value of contour and 2nd is number of points.
  %
  % We need to get each contour separately, and calculate
  % Z for each X,Y.
  i = 1; j = 0;
  limit = size(C,2);
  while (i < limit),

    j = j + 1; contourValues(j) = C(1,i);

    numberPoints = C(2,i);

    endContourSet = i + numberPoints;

    Xc = [ C(1,i+1:endContourSet) ]';
    Yc = [ C(2,i+1:endContourSet) ]';

    %cdata = contourValues(j) + 0*Xc;  % Make cdata the same size as xdata

    % Given each X,Y we project back from 2D to 3D.
    [X3,Y3,Z3] = elec_2d_3d(Xc,Yc,Xrad,Yrad,Zrad);

    if (contourValues(j) < 0)
      % Values < 0 with dashed black line
      if isequal(p.colorMap.style,'Gray')
        line(X3,Y3,Z3,'LineStyle',':','LineWidth',0.5,'Color','black');
      else
        line(X3,Y3,Z3,'LineStyle',':','LineWidth',0.5,'Color','blue');
      end
    elseif (contourValues(j) > 0)
      % Values > 0 with solid black line
      if isequal(p.colorMap.style,'Gray')
        line(X3,Y3,Z3,'LineStyle','-','LineWidth',0.5,'Color','black');
      else
        line(X3,Y3,Z3,'LineStyle','-','LineWidth',0.5,'Color','red');
      end
    else
      % Values = 0 with thicker solid black line
      line(X3,Y3,Z3,'LineStyle','-','LineWidth',0.75,'Color','black');
    end
    i = endContourSet + 1;
  end

  view(0,90), axis tight, rotate3d
  set(gca,'Visible','off');
  %colormap(p.colorMap.map); H = colorbar;
  %absmax = max(abs(contourValues));

  SaveGraphics(fig,'_3D_',p);
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DrawHead(Hradius, Hcolor)

switch Hcolor
  case 'black'
    HCOLOR = [0 0 0];
  case 'white'
    HCOLOR = [1 1 1];
  otherwise
    HCOLOR = [0 0 0];
end
HLINEWIDTH = 30;

rmax = 0.95 * Hradius;

% Plot Head

theta1 = linspace(0,2*pi,50);       % 360 degree rotation
thetad = (theta1(2) - theta1(1))/2; % provide staggered rotation
theta = [theta1, theta1 + thetad];

plot(cos(theta).*rmax,sin(theta).*rmax,'color',HCOLOR,'LineWidth',HLINEWIDTH);

% Plot Nose
width = rmax * 0.1;
tip   = rmax * 1.075;
base  = rmax * 1.005;

plot([width;0;-width],[rmax;tip;rmax],'Color',HCOLOR,'LineWidth',HLINEWIDTH);
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function SaveGraphics(F,type,p)
if ~isequal(p.saveGraphics,'No Save Plots'),

  [path,file,ext] = fileparts(strcat(p.volt.path, filesep, p.volt.file));
  file = strcat(file, type, num2str(p.volt.samplePoint),'.',p.saveGraphics);
  file = fullfile(path,file);
  saveas(F,file);
end
return

author: PatternRecognition

repo: OpenBMI


text:
classdef PartialDFT < spx.dict.Operator

properties(SetAccess=private)
    % Dimensions
    row_pics
    col_perm
    num_rows
    num_cols
end

methods

function self = PartialDFT(row_pics, col_perm)
    if nargin < 2
        error('Row selection and column permutation must be specified');
    end
    self.row_pics = row_pics;
    self.col_perm = col_perm;
    self.num_rows = numel(row_pics);
    self.num_cols = numel(col_perm);
    if self.num_rows > self.num_cols
        error('Number of rows cannot be greater than number of columns.');
    end
end

function [mm, nn]  = get_size(self)
    mm = self.num_rows;
    nn = self.num_cols;
end

function result = apply(self, vectors)
    if size(vectors, 1) ~= self.num_cols
        error('Dimensions mismatch');
    end
    result = fft(vectors(self.col_perm, :)) / sqrt(self.num_cols);
    result = result(self.row_pics, :);
end

function result = apply_ctranspose(self, vectors)
    if size(vectors, 1) ~= self.num_rows
        error('Dimensions mismatch');
    end
    num_vectors = size(vectors, 2);
    full_vectors = zeros(self.num_cols, num_vectors);
    full_vectors(self.row_pics, :) = vectors;
    result = zeros(self.num_cols, num_vectors);
    result(self.col_perm, :) = ifft(full_vectors) * sqrt(self.num_cols);
end

function result = double(self)
    % Converts the operator into a MATRIX
    result = self.apply(eye(self.num_cols));
end

function result = norm(self)
    error('Not implemented');
end

end

end

author: indigits

repo: sparse-plex


text:
function varargout = size(F, dim)
%SIZE   size of a CHEBFUN3V object.
%   D = SIZE(F) returns a four-element vector D = [K, M, N, P]. If F is a
%   column CHEBFUN3V object then K is the number of components in F, and
%   M, N and P are INF. If F is a row vector then K, M and N are INF and P
%   is the number of components in F.
%
%   [K, M, N, P] = SIZE(F) returns the dimensions of F in separate outputs.
%
%   D = SIZE(F, DIM) returns the dimension specified by DIM.
%
% See also CHEBFUN3/SIZE.

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

% Empty check:
if ( isempty( F ) )
    varargout = { [], [], [], [] };
    return
end

nF = F.nComponents;
tr = F.isTransposed;

% Manually work out the size:
if ( ~tr )
    K = nF;
    M = Inf;
    N = Inf;
    P = Inf;
else
    P = nF;
    K = Inf;
    M = Inf;
    N = Inf;
end

% Default to vector:
if ( nargin == 1 )
    dim = 0;
end

% Manually work out what should be displayed:
if ( dim == 1 )
    varargout = {K};
elseif ( dim == 2 )
    varargout = {M};
elseif ( dim == 3 )
    varargout = {N};
elseif ( ( dim == 0 ) && ( nargin == 1 ) )
    if ( nargout <= 1 )
        varargout = {[K, M, N, P]};
    else
        varargout = {K, M, N, P};
    end
else
    error('CHEBFUN:CHEBFUN3V:size:dim', 'Unrecognised dimension.');
end

end

author: chebfun

repo: chebfun

save_path: github-repos/MATLAB/chebfun-chebfun


text:
function [On,Wr] = reorder_mod(W,M)
%REORDER_MOD         Reorder connectivity matrix by modular structure
%
%   On = reorder_mod(W,M);
%   [On Wr] = reorder_mod(W,M);
%
%   This function reorders the connectivity matrix by modular structure and
%   may consequently be useful in visualization of modular structure.
%
%   Inputs:
%       W,      connectivity matrix (binary/weighted undirected/directed)
%       M,      module affiliation vector
%
%   Outputs:
%       On,     new node order
%       Wr,     reordered connectivity matrix
%
%
%   Used in: Rubinov and Sporns (2011) NeuroImage; Zingg et al. (2014) Cell.
%
%
%   2011, Mika Rubinov, UNSW/U Cambridge

%   Modification History:
%   Mar 2011: Original
%	Jan 2015: Improved behavior for directed networks

%#ok<*ASGLU>
%#ok<*AGROW>

W = W+eps;
[u,dum,M] = unique(M);                                      %make consecutive;
n = numel(M);                                               %number of nodes
m = numel(u);                                               %number of modules

Nm=zeros(1,m);                                              %number of nodes in modules
Knm_o=zeros(n,m);                                           %node-to-module out-degree
Knm_i=zeros(n,m);                                           %node-to-module in-degree
for i=1:m
	Nm(i)=nnz(M==i);
    Knm_o(:,i)=sum(W(:,M==i),2);
    Knm_i(:,i)=sum(W(M==i,:),1);
end
Knm=(Knm_o+Knm_i)/2;

Wm=zeros(m);
for u=1:m
    for v=1:m
        Wm(u,v)=sum(sum(W(M==u,M==v)));
    end
end
Bm=(Wm+Wm.')./(2*(Nm.'*Nm));
% Km_o=sum(Wm,2);
% Km_i=sum(Wm,1);
% Bm_oi=Wm-Km_o*Km_i/sum(sum(Wm));
% Bm=(Bm_oi+Bm_oi.')/2;

%1. Arrange densely connected modules together
[I,J,bv]=find(tril(Bm,-1));                             	%symmetrized intermodular connectivity values
[~,ord]=sort(bv,'descend');                                 %sort by greatest relative connectivity
I=I(ord);
J=J(ord);
Om=[I(1) J(1)];                                             %new module order

Si=true(size(I));
Sj=true(size(J));
Si(I==I(1) | I==J(1))=0;
Sj(J==I(1) | J==J(1))=0;

while length(Om)<m                                          %while not all modules ordered
    for u=1:numel(I)
        if Si(u)     && any(J(u)==Om([1 end]))
            old=J(u);
            new=I(u);
            break
        elseif Sj(u) && any(I(u)==Om([1 end]))
            old=I(u);
            new=J(u);
            break
        end
    end
    if old==Om(1);
        Om=[new Om];
    elseif old==Om(end);
        Om=[Om new];
    end
    Si(I==new)=false;
    Sj(J==new)=false;
end

%2. Reorder nodes within modules
On = zeros(n,1,'uint64');                                   %node order array
for i=1:m
    u = Om(i);
    ind = find(M==u);                                       %indices

    mod_imp=[Om; sign((1:m)-i); abs((1:m)-i); Bm(u,Om)].';
    mod_imp=sortrows(mod_imp,[3 -4]);
    mod_imp=prod(mod_imp(2:end,[1 2]),2);

	[dum,ord]=sortrows(Knm(ind,:),mod_imp);                 %sort nodes by number of links to close modules
	On(ind(ord))=1e6*i+(1:Nm(u));                           %assign node order (assumes <1e6 nodes in a module)
end

[dum,On]=sort(On);                                          %reorder nodes
Wr=W(On,On);                                                %reorder matrix

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
function [v] = ind2subv(siz,ndx)
%IND2SUB Multiple subscripts from linear index.
%   IND2SUBV is used to determine the equivalent subscript values
%   corresponding to a given single index into an array.
%
%   [V] = IND2SUBV(SIZ,IND) returns the array V containing the
%   equivalent subscripts corresponding to the index
%   matrix IND for a matrix of size SIZ.

siz = double(siz);
nout = length(siz);
v = nan([numel(ndx),nout]);

if nout > 2
    k = cumprod(siz);
    for i = nout:-1:3,
        vi = rem(ndx-1, k(i-1)) + 1;
        vj = (ndx - vi)/k(i-1) + 1;
        v(:,i) = double(vj);
        ndx = vi;
    end
end

if nout >= 2
    vi = rem(ndx-1, siz(1)) + 1;
    v(:,2) = double((ndx - vi)/siz(1) + 1);
    v(:,1) = double(vi);
else
    v(:,1) = double(ndx);
end

author: kristinbranson

repo: JAABA


text:
function spm_DAiSS
%_______________________________________________________________________
% Copyright (C) 2019 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: spm_DAiSS.m 7755 2019-12-16 13:19:28Z spm $

spm_beamforming;

author: spm


text:
function [index, params, speedup] = flann_build_index(dataset, build_params)
%FLANN_BUILD_INDEX  Builds an index for fast approximate nearest neighbors search
%
% [index, params, speedup] = flann_build_index(dataset, build_params) - Constructs the
% index from the provided 'dataset' and (optionally) computes the optimal parameters.

% Marius Muja, January 2008

    algos = struct( 'linear', 0, 'kdtree', 1, 'kmeans', 2, 'composite', 3, 'kdtree_single', 4, 'hierarchical', 5, 'lsh', 6, 'saved', 254, 'autotuned', 255 );
    center_algos = struct('random', 0, 'gonzales', 1, 'kmeanspp', 2 );
    log_levels = struct('none', 0, 'fatal', 1, 'error', 2, 'warning', 3, 'info', 4);
    function value = id2value(map, id)
        fields = fieldnames(map);
        for i = 1:length(fields),
            val = cell2mat(fields(i));
            if map.(val) == id
                value = val;
                break;
            end
        end
    end
    function id = value2id(map,value)
        id = map.(value);
    end

    default_params = struct('algorithm', 'kdtree' ,'checks', 32, 'eps', 0.0, 'sorted', 1, 'max_neighbors', -1, 'cores', 1, 'trees', 4, 'branching', 32, 'iterations', 5, 'centers_init', 'random', 'cb_index', 0.4, 'target_precision', 0.9,'build_weight', 0.01, 'memory_weight', 0, 'sample_fraction', 0.1, 'log_level', 'warning', 'random_seed', 0);

    if ~isstruct(build_params)
        error('The "build_params" argument must be a structure');
    end

    params = default_params;
    fn = fieldnames(build_params);
    for i = [1:length(fn)],
        name = cell2mat(fn(i));
        params.(name) = build_params.(name);
    end
    if ~isnumeric(params.algorithm),
        params.algorithm = value2id(algos,params.algorithm);
    end
    if ~isnumeric(params.centers_init),
        params.centers_init = value2id(center_algos,params.centers_init);
    end
    if ~isnumeric(params.log_level),
        params.log_level = value2id(log_levels,params.log_level);
    end

    [index, params, speedup] = nearest_neighbors('build_index',dataset, params);

    if isnumeric(params.algorithm),
        params.algorithm = id2value(algos,params.algorithm);
    end
    if isnumeric(params.centers_init),
        params.centers_init = id2value(center_algos,params.centers_init);
    end
end

repo: gspbox


text:
close all
clear

%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=8;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories

save('timecom8','nmpcplot1','impcplot1');
save('feasibility8','nmpcinf','impcinf');

distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF

%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end

function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;

%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];

%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
    x0(4,1)*sin(x0(3,1))*dt;
    0;
    0];
system.C =[0 0 0 0;
    0 0 0 0;
    1*dt 0 0 0;
    0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;

%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;

%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
    fprintf('Run NMPC-DCBF-8\n');
    controller_nmpc_dcbf_multiple8 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple8.obs = obs;
    controller_nmpc_dcbf_multiple8.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple8.tt;
end

function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;

tic;

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
%    error('OSQP did not solve the problem!')
        t2=-1;
        return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)%Iterations for the first time-step
    storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return

x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)%Iterations for the left time steps

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    x0 = ctrlx(1:nx);
    testx0 = [testx0 x0];
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end
end

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end

author: HybridRobotics

repo: NMPC-DCLF-DCBF


text:
function out = ST_SimpleStats(x,whatStat)
% ST_SimpleStats   Basic statistics about an input time series
%
%---INPUTS:
% x, the input time series
%
% whatStat, the statistic to return:
%          (i) 'zcross': the proportionof zero-crossings of the time series
%                        (z-scored input thus returns mean-crossings)
%          (ii) 'maxima': the proportion of the time series that is a local maximum
%          (iii) 'minima': the proportion of the time series that is a local minimum
%          (iv) 'pmcross': the ratio of the number of times that the (ideally
%                          z-scored) time-series crosses +1 (i.e., 1 standard
%                          deviation above the mean) to the number of times
%                          that it crosses -1 (i.e., 1 standard deviation below
%                          the mean)
%          (v) 'zsczcross': the ratio of zero crossings of raw to detrended
%                           time series where the raw has zero mean

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

N = length(x); % length of the time series

switch whatStat
    case 'zcross'
        % Proportion of zero-crossings of the time series
        % (in the case of z-scored input, crosses its mean)
        xch = x(1:end-1).*x(2:end);
        out = sum(xch < 0)/N;

    case 'maxima'
        % Proportion of local maxima in the time series
        dx = diff(x);
        out = sum(dx(1:end-1) > 0 & dx(2:end) < 0)/(N-1);

    case 'minima'
        % Proportion of local minima in the time series
        dx = diff(x);
        out = sum(dx(1:end-1) < 0 & dx(2:end) > 0)/(N-1);

    case 'pmcross'
        % ratio of times cross 1 to -1
        c1sig = sum(BF_SignChange(x-1)); % num times cross 1
        c2sig = sum(BF_SignChange(x+1)); % num times cross -1
        if c2sig == 0
            out = NaN;
        else
            out = c1sig/c2sig;
        end

    case 'zsczcross'
        % ratio of zero crossings of raw to detrended time series
        % where the raw has zero mean
        x = zscore(x);
        xch = x(1:end-1).*x(2:end);
        h1 = sum(xch < 0); % num of zscross of raw series
        y = detrend(x);
        ych = y(1:end-1).*y(2:end);
        h2 = sum(ych < 0); % of detrended series
        if h1 == 0;
            out = NaN;
        else
            out = h2/h1;
        end

    otherwise
        error('Unknown statistic ''%s''',whatStat);
end

end

author: benfulcher

repo: hctsa

save_path: github-repos/MATLAB/benfulcher-hctsa


text:
function D = D2L2R2_updateD(Y, Y_range, D, D_range, X, opts)

    C = numel(Y_range) - 1;
    opts.YXt = true;
    for c = 1 : C
        Mc  = Y  - remove_block_col(D, c, D_range)*remove_block_row(X, c, D_range);
        Yc  = get_block_col(Y, c, Y_range);
        Xc  = get_block_row(X, c, D_range);
        Xcc = get_block_col(Xc, c, Y_range);
        E   = Mc*Xc' + Yc * Xcc';
        F   = 2*Xc*Xc';

        range_Dc = get_range(D_range, c);
        Dc = D(:, range_Dc);
        D(:, range_Dc) = min_rank_dict(Dc, E, F, opts.alpha, opts);
    end
end

author: tiepvupsu

repo: DICTOL

save_path: github-repos/MATLAB/tiepvupsu-DICTOL


text:
function [v2, sigma, p, hpos] = APPestimateVp(lines, imsize, DO_DISPLAY)
% [v2, sigma, p, hpos] = APPestimateVp(lines, imsize, DO_DISPLAY)
% Estimates the principal vanishing points, as in Video Compass [Kosecka
% 2002], except that more (or fewer) than 3 principal vps could be found
%
% Input:
% lines([x1 x2 y1 y2 angle r])
% imsize: size of image
% DO_DISPLAY (optional): whether to create display figures (default=0)
% Output:
% v2(nvp, [x y]) - the found vanishing points; last is outliers; vp are in
%                  units of pixels/imsize(1), with image upper-left at (0,0)
% sigma(nvp) - the variance for each vp
% p(nlines, nvp) - the confidence that each line belongs to each vp
% hpos - horizon position (0 is top of image)
%
% 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 ~exist('DO_DISPLAY')
    DO_DISPLAY = 0;
end

nlines = size(lines, 1);

x1 = [lines(:, [1 3]) ones(size(lines, 1), 1)];
x2 = [lines(:, [2 4]) ones(size(lines, 1), 1)];

% get plane normals for line segments
l = cross(x1, x2);
l = l ./ repmat(sqrt(sum(l.^2,2)), 1, 3);

% make theta range from 0 to pi (instead of -pi to pi)
nbins = 60;
theta = mod(lines(:,5), pi);

% get histogram of thetas
binwidth = pi/nbins;
bincenters = [(binwidth/2):binwidth:(pi-binwidth/2)];
hist_theta = hist(theta, bincenters);

% smooth histogram
for b = 1:nbins
    hist_theta(b) = sum(hist_theta(mod([(b-1):(b+1)]-1, nbins)+1) .* [0.25 0.5 0.25]);
end

% compute curvature of histogram
s = 9;
C = zeros(1, nbins);
for b = 1:nbins
    C(b) = hist_theta(b) - mean(hist_theta(mod([(b-(s-1)/2):(b+(s-1)/2)]-1, nbins)+1));
end

% find dominant peaks surrounded by zero crossings
zc_pos = find((C > 0) & ([C(end) C(1:end-1)]<0));
zc_neg = find((C < 0) & ([C(end) C(1:end-1)]>0));

ngroups = length(zc_pos);
bc = bincenters + pi/nbins/2;
if ngroups>0 && (zc_neg(1) < zc_pos(1))
    i1 = round((zc_pos(end)+zc_neg(end))/2);
    i2 = round((zc_neg(1) + zc_pos(1))/2);
    groups{1} = find((theta > bc(i1)) | (theta < bc(i2)));
    for i = 2:ngroups
        i1 = round((zc_pos(i-1)+zc_neg(i-1))/2);
        i2 = round((zc_neg(i) + zc_pos(i))/2);
        groups{i} = find((theta > bc(i1)) & (theta < bc(i2)));
    end

else
    for i = 1:ngroups
        i1 = ceil((zc_pos(i)+zc_neg(max(i-1,1)))/2);
        i2 = ceil((zc_neg(i) + zc_pos(min(i+1,ngroups)))/2);
        groups{i} = find((theta > bc(i1)) & (theta < bc(i2)));
    end
end

% remove groups with few segments
remove = [];
thresh = max(0.05*nlines, 5);
%thresh = 6;
for i = 1:ngroups
    if length(groups{i}) < thresh
        %disp(num2str([length(groups{i}) nlines]))
        remove(end+1) = i;
    end
end
groups(remove) = [];
ngroups = length(groups);

% initialize EM
sigma = ones(1, ngroups); % sigma = variance here
p = zeros(nlines, ngroups); % p = P(v | l) = P(l|v)P(v)/P(l)
for i = 1:ngroups
    p(groups{i}, i) = 1;
end
%p(:, end) = 1-sum(p(:, 1:end-1), 2);
A = l;
v = [];
sigma = [];
for i = 1:ngroups
    normp = p(:, i) / sum(p(:, i));
    W = diag(normp);
    [eigV, lambda] = eig(A'*W'*W*A);
    [tmp, smallest] = min(diag(lambda));
    v(1:3,i) = eigV(:, smallest);
    sp = sort(normp, 'descend');
    sp = sum(sp(1:min(length(sp), 2)));
    sigma(i) = normp' * (l*v(:,i)).^2 / (1-sum(sp));
end

% create outlier groups
nadd = 3;
ngroups = ngroups + nadd;
sigma(end+(1:nadd)) = 0.2;
tmpv = [0 0 1]';
v(1:3, end+1) = tmpv / sqrt(sum(tmpv.^2));
tmpv = [1 0 1]';
v(1:3, end+1) = tmpv / sqrt(sum(tmpv.^2));
tmpv = [-1 0 1]';
v(1:3, end+1) = tmpv / sqrt(sum(tmpv.^2));
p(:, end+(1:nadd)) = 0;
pv = ones(1, ngroups);

oldv = v;

% do EM
for iter = 1:15
    oldp = p;
    pv = pv / sum(pv);
    % p(l|v) = 1/sqrt(2*pi)/sigma*exp(-(l'v).^2/sigma^2/2)
    S = repmat(sigma, nlines, 1) + 1E-10;
    plv = exp(-(l*v).^2 ./ S / 2) ./ sqrt(S) + 1E-10;
    p = plv .* repmat(pv, nlines, 1);
    p = p ./ repmat(sum(p, 2), 1, ngroups);
    pv = sum(p, 1);
    nmembers = zeros(ngroups,1);
    for i = 1:ngroups
        normp = p(:, i) / sum(p(:, i));
        W = diag(normp);
        [eigV, lambda] = eig(A'*W'*W*A);
        [tmp, smallest] = min(diag(lambda));
        v(1:3,i) = eigV(:, smallest);
        sp = sort(normp, 'descend');
        sp = sum(sp(1:min(length(sp), 2)));
        if (1-sum(sp)) > 0
            sigma(i) = normp' * (l*v(:,i)).^2 / (1-sum(sp));
        else
            sigma(i) = Inf;
        end
    end

    % remove duplicate groups
    remove =[];
    for i = 1:ngroups
        for j = i+1:ngroups
            if (v(1:3, i)'*v(1:3,j) > 0.995) || (pv(i)*nlines <= 3) || (sigma(i) > 10)
                remove(end+1) = i;
                break;
            end
        end
    end
    if length(remove)>0
        p(:, remove) = [];
        sigma(remove) = [];
        v(:, remove) = [];
        pv(remove) = [];
        ngroups = size(p, 2);
    end

    % break when v converges
    if all(size(v)==size(oldv)) && min(diag(oldv'*v)) > 0.999
        break;
    end
    oldv = v;

end

% convert vanishing directions to [x y 1] form with (0,0) in upper-left of
% image
%A = inv([1/imsize(1) 0 -1/imsize(2)/2*imsize(1); 0 1/imsize(1) -1/imsize(1)/2*imsize(1); 0 0 1]);
%v2 = (A*v)';
v2 = v';
v2(:, 3) = v2(:, 3)+1E-4;
% sigma = sigma ./ (v2(:, 3).^2)';
v2 = v2 ./ repmat(v2(:, 3), 1, 3);
v2(:, 1) = v2(:, 1) + imsize(2)/imsize(1)/2;
v2(:, 2) = v2(:, 2) + 1/2;

if nargout > 3
    hpos = APPvp2horizon(v2, sigma, p, imsize);
end
%v2 = v;

if DO_DISPLAY

    figure(1), hold off, plot(bincenters, hist_theta, 'b');
    figure(1), hold on, plot(bincenters, hist_theta, 'r');
    drawnow;

    edge_im = zeros(imsize);
    lines_nnorm(:, [1 2]) = lines(:, [1 2])*imsize(1) + imsize(2)/2;
    lines_nnorm(:, [3 4]) = lines(:, [3 4])*imsize(1) + imsize(1)/2;
    %lines_nnorm = lines;
    for i = 1:length(groups)
        if length(groups{i})>0
            edge_im = draw_line_image2(edge_im, lines_nnorm(groups{i}, 1:4)', i);
        end
    end
    figure(2), imshow(255-label2rgb(edge_im));

    % plot new line memberships
    [tmp, bestv] = max(p, [], 2);
    edge_im = zeros(imsize);
    for i = 1:ngroups
        groups{i} = find(bestv==i);
        if length(groups{i})>0
            edge_im = draw_line_image2(edge_im, lines_nnorm(groups{i}, 1:4)', i);
        end
        %disp(length(groups{i}))
    end
    figure(3), imshow(255-label2rgb(edge_im));
    drawnow;
end

author: Cloud-CV

repo: object-proposals


text:
function [soln,eqn,info] = Poisson3(node,elem,bdFlag,pde,option,varargin)
%% POISSON3 Poisson equation: P1 linear element in 3-D.
%
%   u = Poisson3(node,elem,bdFlag,pde) produces the linear finite element
%   approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
%   The mesh is given by node and elem and the boundary edge is given by
%   bdFlag. See meshdoc, bddoc for details. The data is given by the
%   structure pde which contains function handles f, g_D, g_N, g_R, or d.
%   For general elliptic equations with convection and reaction
%   coefficients, see ellipticpde.
%
% The usage is the same as Poisson. Linear element on a tetrahedron is
% summarized in Poisson3femrate for detail.
%
%   Example
%
%     cubePoisson;
%
%     Poisson3femrate;
%
%   See also Poisson3, squarePoisson, Lshape, crack, mg
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Set up optional input arguments
if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end
if nargin>=6
    HB = varargin{1};
else
    HB = [];
end

N = size(node,1);
NT = size(elem,1);
Ndof = N;

%% Diffusion coefficient
t = cputime;  % record assembling time
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                           % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d) % d is a function
    [lambda,weight] = quadpts3(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxyz = lambda(p,1)*node(elem(:,1),:) ...
			 + lambda(p,2)*node(elem(:,2),:) ...
			 + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
        K = K + weight(p)*pde.d(pxyz);
   end
end

%% Compute geometric quantities and gradient of local basis
[Dphi,volume] = gradbasis3(node,elem);

%% Assemble stiffness matrix
A = sparse(Ndof,Ndof);
for i = 1:4
    for j = i:4
        Aij = dot(Dphi(:,:,i),Dphi(:,:,j),2).*volume;
        if ~isempty(pde.d)
            Aij = K.*Aij;
        end
        if (j==i)
            A = A + sparse(elem(:,i),elem(:,j),Aij,Ndof,Ndof);
        else
            A = A + sparse([elem(:,i);elem(:,j)],[elem(:,j);elem(:,i)],...
                           [Aij; Aij],Ndof,Ndof);
        end
    end
end
clear K Aij

%% Assemble right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if isreal(pde.f) % f is a real number or vector and not a function
   switch length(pde.f)
       case NT  % f is piecewise constant
         bt = pde.f.*volume/4;
         b = accumarray(elem(:),repmat(bt,4,1),[Ndof 1]);
       case N   % f is piecewise linear
         bt = zeros(NT,4);
         bt(:,1) = volume.*(2*pde.f(elem(:,1)) + pde.f(elem(:,2)) ...
                            + pde.f(elem(:,3)) + pde.f(elem(:,4)))/20;
         bt(:,2) = volume.*(2*pde.f(elem(:,2)) + pde.f(elem(:,3)) ...
                            + pde.f(elem(:,1)) + pde.f(elem(:,4)))/20;
         bt(:,3) = volume.*(2*pde.f(elem(:,3)) + pde.f(elem(:,1)) ...
                            + pde.f(elem(:,2)) + pde.f(elem(:,4)))/20;
         bt(:,4) = volume.*(2*pde.f(elem(:,4)) + pde.f(elem(:,1)) ...
                            + pde.f(elem(:,2)) + pde.f(elem(:,3)))/20;
         b = accumarray(elem(:),bt(:),[Ndof 1]);
       case 1   % f is a scalar e.g. f = 1
         bt = pde.f*volume/4;
         b = accumarray(elem(:),repmat(bt,4,1),[Ndof 1]);
   end
end
if ~isempty(pde.f) && ~isreal(pde.f)  % f is a function
    [lambda,weight] = quadpts3(option.fquadorder);
    phi = lambda;                 % linear bases
	nQuad = size(lambda,1);
    bt = zeros(NT,4);
    for p = 1:nQuad
		% quadrature points in the x-y-z coordinate
		pxyz = lambda(p,1)*node(elem(:,1),:) ...
			 + lambda(p,2)*node(elem(:,2),:) ...
			 + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
		fp = pde.f(pxyz);
        for j = 1:4
            bt(:,j) = bt(:,j) + weight(p)*phi(p,j)*fp;
        end
    end
    bt = bt.*repmat(volume,1,4);
    b = accumarray(elem(:),bt(:),[Ndof 1]);
end
clear pxyz bt

%% Set up boundary conditions
[AD,b,u,freeNode,isPureNeumann] = getbd3(b);

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

%% Solve the system of linear equations
if isempty(freeNode), return; end
% Set up solver
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
if isPureNeumann
    option.solver = 'mg';
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        t = cputime;
        u(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',cputime - t,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        if ~isfield(option,'mgoption')   % no option.mgoption
            option.mgoption.x0 = u;
            option.mgoption.solver = 'CG';
        end
        [u,info] = mg(AD,b,elem,option.mgoption,HB);
    case 'amg'
        if ~isfield(option,'amgoption')  % no option.amgoption
            option.amgoption.x0 = u;
            option.amgoption.solver = 'CG';
        end
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option.amgoption);
end
% post-process for pure Neumann problem
if isPureNeumann
    patchVolume = accumarray(elem(:),repmat(volume/4,4,1), [N 1]);
    uc = sum(u.*patchVolume)/sum(volume);
    u = u - uc;   % int u = 0
end

%% Compute Du
dudx = u(elem(:,1)).*Dphi(:,1,1)+u(elem(:,2)).*Dphi(:,1,2) ...
      +u(elem(:,3)).*Dphi(:,1,3)+u(elem(:,4)).*Dphi(:,1,4);
dudy = u(elem(:,1)).*Dphi(:,2,1)+u(elem(:,2)).*Dphi(:,2,2) ...
      +u(elem(:,3)).*Dphi(:,2,3)+u(elem(:,4)).*Dphi(:,2,4);
dudz = u(elem(:,1)).*Dphi(:,3,1)+u(elem(:,2)).*Dphi(:,3,2) ...
      +u(elem(:,3)).*Dphi(:,3,3)+u(elem(:,4)).*Dphi(:,3,4);
Du = [dudx, dudy, dudz];

%% Output
if nargout == 1
    soln = u;
else
    soln = struct('u',u,'Du',Du);
    eqn = struct('A',AD,'b',b,'freeNode',freeNode,'Lap',A);
    info.assembleTime = assembleTime;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeNode,isPureNeumann] = getbd3(b)
    %% Set up of boundary conditions.
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedNode) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % zero mean value. To avoid a singular matrix, the 1st node is chosen as
    % fixedNode.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.
    %
    % Reference: Long Chen. Finite Element Methods and its Programming. Lecture
    % Notes.

    u = zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    % Find Robin face dofs
    Robin = [];
    isRobin = (bdFlag(:) == 3);
    if any(isRobin)
        allFace = [elem(:,[2 4 3]);elem(:,[1 3 4]);elem(:,[1 4 2]);elem(:,[1 2 3])];
        Robin = allFace(isRobin,:);
    end
    % Assemble the mass matrix for Robin boundary condition
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        v12 = node(Robin(:,2),:)-node(Robin(:,1),:);
        v13 = node(Robin(:,3),:)-node(Robin(:,1),:);
        area = 0.5*sqrt(abs(sum(mycross(v12,v13,2).^2,2)));
        if ~isfield(option,'gRquadorder')
            option.gRquadorder = 3;
        end
        [lambdagR,weightgR] = quadpts(option.gRquadorder);
        nQuadgR = size(lambdagR,1);
        bdphi = lambdagR;
        NR = size(Robin,1);
        ss = zeros(NR,3,3);
        % int g_R phi_i phi_j
        for pp = 1:nQuadgR
            ppxyz = lambdagR(pp,1)*node(Robin(:,1),:) ...
                  + lambdagR(pp,2)*node(Robin(:,2),:) ...
                  + lambdagR(pp,3)*node(Robin(:,3),:);
            gRp = pde.g_R(ppxyz);
            for iR = 1:3
                for jR = iR:3
                    ss(:,iR,jR) = ss(:,iR,jR) + ...
                    weightgR(pp)*gRp*bdphi(pp,iR).*bdphi(pp,jR);
                end
            end
        end
        ss(:) = ss(:).*repmat(area,9,1);
        % assemble
        index = 0;
        for iR = 1:3
            for jR = 1:3
                iiR(index+1:index+NR) = double(Robin(:,iR));
                jjR(index+1:index+NR) = double(Robin(:,jR));
                if jR>=iR
                    ssR(index+1:index+NR) = ss(:,iR,jR);
                else
                    ssR(index+1:index+NR) = ss(:,jR,iR);
                end
                index = index + NR;
            end
        end
        A = A + sparse(iiR,jjR,ssR,Ndof,Ndof);
    end

    % Find Dirichlet boundary nodes: fixedNode
    fixedNode = [];
    freeNode = [];
    if ~isempty(bdFlag) % bdFlag specifies different bd conditions
        [fixedNode,bdFace,isBdNode] = findboundary3(elem,bdFlag);
        freeNode = ~isBdNode;
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given
        [fixedNode,bdFace,isBdNode] = findboundary3(elem);
        freeNode = ~isBdNode;
    end

    % Modify the matrix for different boundary conditions
    AD = A;
    % Dirichlet boundary condition
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    if ~isempty(fixedNode)
        bdidx = zeros(Ndof,1);
        bdidx(fixedNode) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    end
    % Neumann boundary condition
    isPureNeumann = false;
    if isempty(fixedNode) && isempty(Robin) % pure Neumann boundary condition
        isPureNeumann = true;
        AD(1,1) = AD(1,1) + 1e-6; % eliminate the kernel
%         fixedNode = 1;
%         freeNode = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end

    %% Part 2: Find boundary faces and modify the load b
    % Find boundary faces: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        Neumann = bdFace;
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        [tempvar,Neumann] = findboundary3(elem); %#ok<ASGLU> %TODO: is this findboundary3?
    end
    % Neumann boundary condition
    if ~isempty(Neumann) && ~isempty(pde.g_N) && ~(isnumeric(pde.g_N) && all(pde.g_N == 0))
        v12 = node(Neumann(:,2),:)-node(Neumann(:,1),:);
        v13 = node(Neumann(:,3),:)-node(Neumann(:,1),:);
        area = 0.5*sqrt(abs(sum(mycross(v12,v13,2).^2,2)));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 2;
        end
        [lambdagN,weightgN] = quadpts(option.gNquadorder);
        phigN = lambdagN;                 % linear bases
        nQuadgN = size(lambdagN,1);
        gf = zeros(size(Neumann,1),3);
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxyz = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                  + lambdagN(pp,2)*node(Neumann(:,2),:) ...
                  + lambdagN(pp,3)*node(Neumann(:,3),:);
            gNp = pde.g_N(ppxyz);
            for iN = 1:3
                gf(:,iN) = gf(:,iN) + weightgN(pp)*phigN(pp,iN)*gNp;
            end
        end
        gf = gf.*repmat(area,1,3);
        b = b + accumarray(Neumann(:),gf(:),[Ndof,1]);
    end
    % The case with non-empty Neumann edges but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann edges and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if ~isPureNeumann && ~isempty(fixedNode) && ...
       ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && all(pde.g_D == 0))    % nonzero g_D
        if isnumeric(pde.g_D)
            u(fixedNode) = pde.g_D(fixedNode);
        else
            u(fixedNode) = pde.g_D(node(fixedNode,:));
        end
        b = b - A*u;
    end
    if ~isPureNeumann && ~isempty(fixedNode) % non-empty Dirichlet boundary condition
        b(fixedNode) = u(fixedNode);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b); % compatiable condition (f,1) + <gN,1> = 0
%         b(1) = 0;        % 1 is fixedNode and set u(1) = 0
    end
    end % end of getbd3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end % end of Poisson3

repo: ifem


text:
function sD = som_denormalize(sD,varargin)

%SOM_DENORMALIZE Denormalize data.
%
% sS = som_denormalize(sS, [argID, value, ...])
%
%   sS = som_denormalize(sS)
%   sS = som_denormalize(sS,[1:3 10],'remove')
%    D = som_denormalize(D,sM.comp_norm)
%    D = som_denormalize(D,sM,[1:3 10])
%
%  Input and output arguments ([]'s are optional):
%   sS                The data to which the denormalization is applied.
%                     The modified and updated data is returned.
%            (struct) data or map struct
%            (matrix) data matrix (a matrix is also returned)
%   [argID, (string) See below. The values which are unambiguous can
%    value] (varies) be given without the preceeding argID.
%
% Here are the valid argument IDs and corresponding values. The values which
% are unambiguous (marked with '*') can be given without the preceeding argID.
%   'norm'   *(struct) Normalization struct, or an array of such.
%                      Alternatively, a map/data struct can be given
%                      in which case its '.comp_norm' field is used
%                      (see below).
%            *(cell array) Of normalization structs. Typically, the
%                      '.comp_norm' field of a map/data struct. The
%                      length of the array must be equal to data dimension.
%   'remove' *(string) If 'remove' tag is specified, the
%                      normalization operations are not only undone,
%                      they are also removed from the struct.
%   'comps'  *(vector) the components to which the denormalization is
%                      applied, default is [1:dim] ie. all components
%
% For more help, try 'type som_denormalize' or check out online documentation.
% See also SOM_NORMALIZE, SOM_NORM_VARIABLE, SOM_INFO.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% som_denormalize
%
% PURPOSE
%
% Undo normalizations of data structs/sets.
%
% SYNTAX
%
%  sS = som_denormalize(sS)
%  sS = som_denormalize(...,'argID',value,...);
%  sS = som_denormalize(...,value,...);
%   D = som_denormalize(D,sNorm)
%
% DESCRIPTION
%
% This function is used to undo normalizations of data structs/sets. If a
% data/map struct is given, all normalizations in the '.comp_norm' field are
% undone and, thus, the values in the original data context are returned. If
% a matrix is given, the normalizations to undo must be given as the second
% argument. SOM_DENORMALIZE actually uses function SOM_NORM_VARIABLE to
% handle the normalization operations, and only handles the data struct/set
% specific stuff itself.
%
% Normalizations are always one-variable operations. In the data and map
% structs the normalization information for each component is saved in the
% '.comp_norm' field, which is a cell array of length dim. Each cell
% contains normalizations for one vector component in a
% struct array of normalization structs. Each component may have different
% amounts of different kinds of normalizations. Typically, all
% normalizations are either 'undone' or 'done', but in special situations
% this may not be the case. The easiest way to check out the status of the
% normalizations is to use function SOM_INFO, e.g. som_info(sS,3)
%
% REQUIRED INPUT ARGUMENTS
%
%   sS                The data to which the denormalization is applied.
%            (struct) Data or map struct. The normalizations in the
%                     '.comp_norm' field are undone for the specified
%                     components.
%            (matrix) Data matrix. The normalization to undo must be
%                     given in the second argument.
%
% OPTIONAL INPUT ARGUMENTS
%
%  argID (string) Argument identifier string (see below).
%  value (varies) Value for the argument (see below).
%
%  The optional arguments can be given as 'argID',value -pairs. If an
%  argument is given value multiple times, the last one is used. The
%  valid IDs and corresponding values are listed below. The values
%  which are unambiguous (marked with '*') can be given without the
%  preceeding argID.
%
%   sNorm    *(struct) Normalization struct, or an array of structs, which
%                      is undone for all specified components. If the
%                      '.status' field of the struct(s) is 'uninit',
%                      the undoing operation is interrupted - it cannot be
%                      done. Alternatively, the struct may be map or
%                      data struct in which case its '.comp_norm' field
%                      is used (see the cell array option below).
%            *(cell array) In practice, the '.comp_norm' field of
%                      a data/map struct. The length of the array
%                      must be equal to the dimension of the given
%                      data set (sS). Each cell contains the
%                      normalization(s) for one component. Only the
%                       normalizations listed in comps argument are
%                      undone though.
%
%   'remove' *(string) If 'remove' tag is specified, the
%                      normalization operations are not only undone,
%                      they are also removed from the struct.
%
%   'comps'  *(vector) The components which are denormalized.
%                      Default is to undo all components.
%            *(string) 'all'
%
% OUTPUT ARGUMENTS
%
%   sS                Modified and/or updated data.
%            (struct) If a struct was given as input argument, the
%                     same struct is returned with denormalized data and
%                     updated '.comp_norm' fields.
%            (matrix) If a matrix was given as input argument, the
%                     denormalized data matrix is returned.
%
% EXAMPLES
%
%  To undo normalization of a data/map struct:
%
%    sD = som_denormalize(sD);
%    sM = som_denormalize(sM);
%
%  To completely remove the normalizations, use the 'remove' tag:
%
%    sD = som_denormalize(sD,'remove');
%
%  To undo only a few selected components, use the comps argument:
%
%    sD = som_denormalize(sD,[1 3:5]);
%
%  To denormalize a set of values from a data set D (which must be
%  of equal dimension as the data in sD):
%
%    D = som_denormalize(D,sD);
%  or
%    D = som_denormalize(D,sD.comp_norm);
%  only denormalize a few components
%    D = som_denormalize(D,sD,[1 3:5]);
%
%  Assuming you have a few values of a certain vector component (i)
%  in a vector (x) which you want to denormalize:
%
%    xorig = som_denormalize(x,sD.comp_norm{i});
%  or using SOM_NORM_VARIABLE
%    xorig = som_norm_variable(x,sD.comp_norm{i},'undo');
%
%  To check out the status of normalization in a struct use SOM_INFO:
%
%    som_info(sM,3)
%    som_info(sD,3)
%
% SEE ALSO
%
%  som_normalize      Add/apply/redo normalizations of a data struct/set.
%  som_norm_variable  Normalization operations for a set of scalar values.
%  som_info           User-friendly information of SOM Toolbox structs.

% Copyright (c) 1998-2000 by the SOM toolbox programming team.
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 2.0beta juuso 151199 150300

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% check arguments

error(nargchk(1, 3, nargin));  % check no. of input arguments is correct

% sD
struct_mode = isstruct(sD);
if struct_mode,
  switch sD.type
   case 'som_map', D = sD.codebook;
   case 'som_data', D = sD.data;
   otherwise, error('Illegal struct.')
  end
else
  D = sD;
end
[dlen dim] = size(D);

% varargin
comps = [1:dim];
remove_tag = 0;
if struct_mode, sNorm = sD.comp_norm; else sNorm = []; end
i=1;
while i<=length(varargin),
  argok = 1;
  if ischar(varargin{i}),
    switch varargin{i},
     % argument IDs
     case 'comps', i=i+1; comps = varargin{i};
     case {'norm','sNorm','som_norm'}, i=i+1; sNorm = varargin{i};
     % unambiguous values
     case 'remove', remove_tag = 1;
     otherwise argok=0;
    end
  elseif isnumeric(varargin{i}),
    comps = varargin{i};
  elseif isstruct(varargin{i}),
    sNorm = varargin{i};
  elseif iscell(varargin{i}),
    sNorm = varargin{i};
  else
    argok = 0;
  end
  if ~argok,
    disp(['(som_denormalize) Ignoring invalid argument #' num2str(i+1)]);
  end
  i = i+1;
end

% check comps
if ischar(comps), comps = [1:dim]; end
if isempty(comps), return; end
if size(comps,1)>1, comps = comps'; end  % make it a row vector

% sNorm
% check out the given normalization
% (and if necessary, copy it for each specified component)
if isstruct(sNorm),
  switch sNorm(1).type,
   case {'som_map','som_data'}, csNorm = sNorm(1).comp_norm;
   case {'som_norm'}, for i=comps, csNorm{i} = sNorm; end
   otherwise,
    error('Invalid struct for sNorm.')
  end
elseif iscell(sNorm),
  csNorm = sNorm;
else
  error('Illegal value for sNorm.')
end

% check that csNorm and comps possibly agree
if max(comps) > length(csNorm),
  error('Given normalizations does not match the components.')
end
if length(csNorm) ~= dim,
  error('Given normalizations does not match data dimension.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% action

% undo the normalizations
for i = comps,
  len = length(csNorm{i});
  for j=len:-1:1,
    sN = csNorm{i}(j);
    if struct_mode,
      if strcmp(sN.status,'done'),
	[x,sN] = som_norm_variable(D(:,i), sN, 'undo');
	D(:,i) = x;
	csNorm{i}(j) = sN;
      end
    else
      D(:,i) = som_norm_variable(D(:,i), sN, 'undo');
    end
  end
end

% remove normalizations
if struct_mode && remove_tag,
  for i = comps, csNorm{i} = []; end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% output

if struct_mode,
  switch sD.type
   case 'som_map', sD.codebook = D;
   case 'som_data', sD.data = D;
   otherwise, error('Illegal struct.')
  end
  sD.comp_norm = csNorm;
else
  sD = D;
end

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

author: ilarinieminen

repo: SOM-Toolbox


text:
function varargout = graphicalComparison(keyword, planC, optS, varargin);
%Graphical Comparison Tool, displays given metrics and a linear aggregate based on weights.
%JRA 6.4.03
%JRA 6.6.03 added alot of stuff, mode change etc.
%JRA 6.9.03 units
%went to CVS
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

global gCState;
uicolor              = [.9 .9 .9];
units = 'normalized';
nargout = 0;

switch lower(keyword)
    case 'init'
        gCState = [];
        gCState.metrics = [];
        gCState.optS.mode='bar';
        gCState.optS.units='on';
        %Get figure, set its size based on screen size.
        screenSize = get(0,'ScreenSize');
        h = figure('Position', [screenSize(3)/10 screenSize(4)/10 screenSize(3)/10*8 screenSize(4)/10*6]);
        try
            global stateS
            stateS.handle.graphicalComparisonFig = h;
        end
        set(h, 'NumberTitle', 'off');
        set(h, 'Name', 'Graphical Comparison of Metrics');
        set(h, 'MenuBar', 'none');
        gCState.handles.figure = h;

        %Specify plot dimensions
        gCState.graphics.plotWidth = .9;
        gCState.graphics.plotHeight = .7;
        gCState.graphics.plotLeft = (1 - gCState.graphics.plotWidth)/2;
        gCState.graphics.plotBottom = .2;

        %Specify other graphic positions
        gCState.graphics.buttonBottom = .155;
        gCState.graphics.buttonWidth =.150;
        gCState.graphics.buttonHeight = .035;
        pause(.01);

    case 'mode' %interface to change from bar to line mode.
        if(length(varargin) ~= 0)
            if(lower(varargin{1}) == 'bar')
                gCState.optS.mode='bar';
            elseif(lower(varargin{1}) == 'line')
                gCState.optS.mode='line';
            end
        else
            if(strcmpi(gCState.optS.mode,'bar'))
                gCState.optS.mode='line';
            else
                gCState.optS.mode='bar';
            end
        end
        graphicalComparison('draw');

    case 'toggleunits' %interface to toggle units
        if(length(varargin) ~= 0)
            if(lower(varargin{1}) == 'on')
                gCState.optS.units='on';
            elseif(lower(varargin{1}) == 'off')
                gCState.optS.units='off';
            end
        else
            if(strcmpi(gCState.optS.units,'on'))
                gCState.optS.units='off';
            else
                gCState.optS.units='on';
            end
        end
        graphicalComparison('units');

    case 'add' %interface to add metric functions, takes cell array of metrics
        if(nargin == 5)
            gCState.plans.names = varargin{2};
        else
            gCState.plans.names = [];
        end
        planMetricsS = varargin{1};
        numMetrics = length(planMetricsS);

        %Evaluate metrics, data is stored in metric's valueV field.
        statusBar = waitbar(0,'Calculating/Retrieving Metric Data');
        for i=1:numMetrics
            [planC, planMetricsS(i)] = feval(planMetricsS(i).functionName, planC, planMetricsS(i), 'evaluate');
            waitbar(i/numMetrics,statusBar)
        end
        close(statusBar)

        %Store metrics locally
        gCState.metrics = planMetricsS;
        graphicalComparison('refresh');
        nargout = 1;
        varargout{1} = planC;
        return;

    case 'refresh'
        figure(gCState.handles.figure);
        numMetrics = length(gCState.metrics);
        numPlans = length(gCState.metrics(1).doseSets);

        %Normalize the metrics on a scale of 0-1, store under normalizedMetrics. Range values stored in metrics are used.
        normalizedMetrics = [];
        normalizedPlan = [];
        for i=1:numMetrics
            if gCState.metrics(i).range(1) == inf;
                minValue = max(gCState.metrics(i).valueV);
            elseif gCState.metrics(i).range(1) == -inf;
                minValue = min(gCState.metrics(i).valueV);
            else
                minValue = gCState.metrics(i).range(1);
            end
            if gCState.metrics(i).range(2) == inf;
                maxValue = max(gCState.metrics(i).valueV);
            elseif gCState.metrics(i).range(2) == -inf;
                maxValue = min(gCState.metrics(i).valueV);
            else
                maxValue = gCState.metrics(i).range(2);
            end
            metricRange = maxValue - minValue;
            for j=1:length(gCState.metrics(i).valueV)
                if metricRange == 0
                    relativeValue = .5; %If all values are equal, set to .5.
                else
                    relativeValue = (gCState.metrics(i).valueV(j) - minValue) / metricRange;
                end
                normalizedPlan = [normalizedPlan;relativeValue];
            end
            normalizedMetrics = [normalizedMetrics normalizedPlan];
            normalizedPlan = [];
        end
        gCState.normalizedMetrics = normalizedMetrics;
        weights = ones(numMetrics, 1);
        gCState.weights = weights;
        %

        hV = findobj('tag', 'metricWeight');
        delete(hV);
        hV = findobj('tag', 'aggregate');
        delete(hV);

        %Set up graphics stuff, including labels and weight boxes
        hV = findobj('tag', 'ComparisonAxes');
        delete(hV);
        gCState.handles.axes = axes('units', units, 'Position', [gCState.graphics.plotLeft, gCState.graphics.plotBottom, gCState.graphics.plotWidth, gCState.graphics.plotHeight], 'tag', 'ComparisonAxes');
        gCState.handles.weights = [];

        %Clear previous Graphics
        hV = findobj('tag', 'metricWeight');
        delete(hV);
        hV = findobj('tag', 'metricNameTag');
        delete(hV);
        hV = findobj('tag', 'aggregate');
        delete(hV);
        hV = findobj('tag', 'modeToggle');
        delete(hV);
        hV = findobj('tag', 'unitToggle');
        delete(hV);

        widthOfWorkingArea = gCState.graphics.plotWidth;
        weightButtonYValue = gCState.graphics.buttonBottom;
        weightButtonWidth = min(gCState.graphics.buttonWidth, widthOfWorkingArea/(numMetrics+1) - .01);
        weightButtonHeight = gCState.graphics.buttonHeight;
        %Draw labels and weight fields for all but aggregate
        for i=1:numMetrics
            weightButtonXValue = widthOfWorkingArea/(numMetrics+1)/2 + (i-1)*widthOfWorkingArea/(numMetrics+1) + gCState.graphics.plotLeft - weightButtonWidth/2;

            gCState.handles.weights = [gCState.handles.weights uicontrol('units',units,'BackgroundColor',uicolor,'Position',[weightButtonXValue weightButtonYValue weightButtonWidth weightButtonHeight], ...
                'String',weights(i),'Style','edit','Tag','metricWeight','callback','graphicalComparison(''weightChange'');','tooltipstring','Change Weight of this Metric')];

            uicontrol('units',units,'BackgroundColor',uicolor,'Position',[weightButtonXValue weightButtonYValue-weightButtonHeight weightButtonWidth weightButtonHeight],'String',gCState.metrics(i).name, ...
                'Style','text','Tag','metricNameTag');
            uicontrol('units',units,'BackgroundColor',uicolor,'Position',[weightButtonXValue weightButtonYValue-2*weightButtonHeight weightButtonWidth weightButtonHeight],'String',gCState.metrics(i).units, ...
                'Style','text','Tag','metricNameTag');
            uicontrol('units',units,'BackgroundColor',uicolor,'Position',[weightButtonXValue weightButtonYValue-3*weightButtonHeight weightButtonWidth weightButtonHeight],'String',gCState.metrics(i).note, ...
                'Style','text','Tag','metricNameTag');
        end
        %Draw labels and weight fields for aggregate
        uicontrol('units',units,'BackgroundColor',uicolor, ...
            'Position',[weightButtonXValue+widthOfWorkingArea/(numMetrics+1) weightButtonYValue weightButtonWidth weightButtonHeight],'String','Aggregate', ...
            'Style','text','Tag','aggregate');

        uicontrol('units',units,'BackgroundColor',uicolor, ...
            'Position',[weightButtonXValue+widthOfWorkingArea/(numMetrics+1) weightButtonYValue-gCState.graphics.plotLeft weightButtonWidth weightButtonHeight],'String','Mode', ...
            'Style','pushbutton','Tag','modeToggle', 'callback', 'graphicalComparison(''mode'');');
        uicontrol('units',units,'BackgroundColor',uicolor, ...
            'Position',[weightButtonXValue+widthOfWorkingArea/(numMetrics+1) weightButtonYValue-2*gCState.graphics.plotLeft weightButtonWidth weightButtonHeight],'String','Values', ...
            'Style','pushbutton','Tag','unitToggle', 'callback', 'graphicalComparison(''toggleunits'');');

        %Call the function to draw lines/bars and labels.
        graphicalComparison('draw');

        %Draw graphicalComparison lines/bars.
    case 'draw'
        numMetrics = length(gCState.metrics);
        widthOfWorkingArea = gCState.graphics.plotWidth;
        figure(gCState.handles.figure);
        %Delete old bars/lines
        hV = findobj('tag', 'comparisonBar');
        delete(hV)
        hV = findobj('tag', 'comparisonLine');
        delete(hV)
        %end
        normalizedMetrics = gCState.normalizedMetrics;
        if max(cumsum(gCState.weights)) ~= 0
            normalizedWeights = gCState.weights / max(cumsum(gCState.weights));
        else
            normalizedWeights = gCState.weights * 0;
        end
        aggregate = normalizedMetrics * normalizedWeights;
        axis off;
        %Draw bars, and set callbacks/tags
        if strcmpi(gCState.optS.mode,'bar')
            gCState.handles.bars = bar([normalizedMetrics';aggregate']);
            for i=1:length(gCState.handles.bars)
                set(gCState.handles.bars(i), 'ButtonDownFcn', ['graphicalComparison(''test'',' num2str(i) ');'], 'Tag', 'comparisonBar');
            end
            axis([.5 numMetrics+1.5 0 1])
            %Draw lines, and set callbacks/tags
        elseif strcmpi(gCState.optS.mode,'line')
            gCState.handles.lines = plot([normalizedMetrics';aggregate'], 'Marker', 'o', 'Tag', 'comparisonLine', 'LineWidth', 2);
            for i=1:length(gCState.handles.lines)
                set(gCState.handles.lines(i), 'ButtonDownFcn', ['graphicalComparison(''test'',' num2str(i) ');'], 'Tag', 'comparisonLine');
            end
            axis([.5 numMetrics+1.5 0 1])
        end
        refresh;
        legend(gCState.plans.names) %legend placement is up in the air
        graphicalComparison('units')

        %Draw (or erase) values in graph for either lines or bars
    case 'units'
        hV = findobj('tag', 'metricValues');
        delete(hV)
        if(strcmpi(gCState.optS.units,'off'))
            return;
        end
        numMetrics = length(gCState.metrics);
        yValues = gCState.normalizedMetrics;
        if(strcmpi(gCState.optS.mode,'line'))
            numLines = length(gCState.handles.lines);
            for i=1:numMetrics
                for j=1:numLines
                    text((1/(numMetrics+1))*(i-1) + 1/(numMetrics+1)/2+.01, yValues(j,i), num2str(gCState.metrics(i).valueV(j)), 'FontSize', 8,'units', units, 'Tag', 'metricValues');
                end
            end
        elseif(strcmpi(gCState.optS.mode,'bar'))
            numBars = length(gCState.handles.bars);
            for i=1:numMetrics
                for j=1:numBars
                    if(numBars*numMetrics > 9) %use vertical text if too many bars.
                        text(i - (numBars/2)*1/(numBars+1.9) + (j-1)*1/(numBars+1.5) + 1/(numBars+1.9)/2, yValues(j,i) + .01, num2str(gCState.metrics(i).valueV(j)), 'FontSize', 8, 'Tag', 'metricValues', 'Rotation', 90);
                    else
                        text(i - (numBars/2)*1/(numBars+1.9) + (j-1)*1/(numBars+1.5), yValues(j,i) + .02, num2str(gCState.metrics(i).valueV(j)), 'FontSize', 8, 'Tag', 'metricValues', 'Rotation', 0);
                    end
                end
            end
        end

        %Weight change callback.
    case 'weightchange'
        gCState.weights = [];
        for i=1:length(gCState.handles.weights)
            weight = str2num(get(gCState.handles.weights(i), 'String'));
            if isempty(weight)|weight == 0
                warndlg('Please enter weights as some finite value');
                return;
            end
            gCState.weights = [gCState.weights weight];
        end
        gCState.weights = gCState.weights';
        graphicalComparison('draw');

    case 'test'
        if isempty(varargin)
            return
        end
        planNum = varargin{1}
        pointerMatrix = get(gCState.handles.axes, 'CurrentPoint');
        metricNum = round(pointerMatrix(1,1))
end

author: cerr

repo: CERR

save_path: github-repos/MATLAB/cerr-CERR


text:
% Chi^2 histogram distance. A,B are matrices of example data
% vectors, one per column. The distance is sum_i
% (u_i-v_i)^2/(u_i+v_i+epsilon). The output distance matrix is
% (#examples in A)x(#examples in B)

function D = dist_chi2(A,B,epsilon)

if nargin<3, epsilon=1e-100; end
[d,m]=size(A);
[d1,n]=size(B);
if (d ~= d1)
    error('column length of A (%d) != column length of B (%d)\n',d,d1);
end

% With the MATLAB JIT compiler the trivial implementation turns out
% to be the fastest, especially for large matrices.
D = zeros(m,n);
for i=1:m % m is number of samples of A
    for j=1:n % n is number of samples of B
        D(i,j) = sum((A(:,i)-B(:,j)).^2./(A(:,i)+B(:,j)+epsilon));
    end
end
%end

author: kittenish

repo: Image-Shadow-Detection-and-Removal


text:
function dat = sensitivity(algo,dat)

  [x y]=get_xy(dat);
  if length(find(abs(x)==1))~=prod(size(x))
    disp('[warning: sign not taken before class_loss]');
    x=sign(x);
  end;

  tPos=sum(y==1 & x==1);
  tNeg=sum(y==-1 & x==-1);
  fPos=sum(y==-1 & x==1);
  fNeg=sum(y==1 & x==-1);

  lss=tPos/(tPos+fNeg);

  dat=data([get_name(dat) ' -> sensitivity=' num2str(lss,4) ],[],lss);

author: canlab

repo: CanlabCore

save_path: github-repos/MATLAB/canlab-CanlabCore


text:
function [f,dfdx,dfdu,dfdxdu] = spm_fx_mfm_D(x,u,P,M)
% state equations for neural-mass and mean-field models (delay version)
% FORMAT [f,dfdx,dfdu,dfdxdu] = spm_fx_mfm(x,u,P,M)
%
% x         - states (means and covariances)
% dfdx,...  - derivatives with repect to x and u
%
% x{1}(i,j,k)   - k-th state of j-th population on i-th source
% x{2}(:,:,i,j) - covariance among k states
%
%   population: 1 - excitatory spiny stellate cells (input cells)
%               2 - inhibitory interneurons
%               3 - excitatory pyramidal cells      (output cells)
%
%        state: 1 V  - voltage
%               2 gE - conductance (excitatory)
%               3 gI - conductance (inhibitory)
%
%--------------------------------------------------------------------------
% refs:
%
% This routine is exactly the same as spm_fx_mfm but premultiplies the flow
% with the delay operator to return the flow on delayed states. This is
% necessary for accurate computation of the Jacobian under steady state
% assumptions
%
% Delays
%==========================================================================
% Delay differential equations can be integrated efficiently (but
% approximately) by absorbing the delay operator into the Jacobian
%
%    f(d) = dx(t)/dt = f(x(t - d))
%                    = Q(d)f(x(t))
%
%    J(d)            = Q(d)df/dx
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_mfm_D.m 2393 2008-10-23 14:58:50Z karl $

% get delyed flow and Jacobian
%--------------------------------------------------------------------------
[f,J,Q] = spm_fx_mfm(x,u,P,M);
f       = Q*f;
J       = Q*J;