%% DEMO_mesh_bifurcation_cut_loft_branch
% This demo shows how one can use logic operations to cut meshes and to
% extrude features from meshes. Here a bifurcated vessel is created by
% cutting a hole in a particular direction and by extruding the side-branch
% from this hole. The initial surface mesh is then thickened to produce a
% hexahedral mesh.

%%

clear; close all; clc;

%%
% Plot Settings
fontSize=15;
faceAlpha=0.8;
edgeColor=0.*ones(1,3);
lineWidth1=2;
markerSize1=50;
markerSize2=25;

%% Control parameters
pointSpacing=0.5;
trunkRadiusInner=3; %Radius
trunkLength=15; %Length
branchRadiusInner=2;
branchLength=6;
branchAngle=50;
cutDist=0.5;
branchBaseOffset=cutDist+pointSpacing;

wallThickness=0.5;
numElementsWall=2;

bezierTangency=0.5;

numSmootIterations=25;
smoothMethod='HC';
smoothIncludeSteps=2;

%% Derived parameters

%Get outer radii
trunkRadiusOuter=trunkRadiusInner+wallThickness;
branchRadiusOuter=branchRadiusInner+wallThickness;

%Have branch exit in middle
V_branch_origin=[0 0 -trunkRadiusOuter*tand(90-branchAngle)];

%% Create example geometry

%Guide curves and branches
R=euler2DCM([0 (branchAngle/180)*pi 0]);
R_90=euler2DCM([0 (90/180)*pi 0]);

%Main trunk
Vm=evenlySpaceCurve([0 0 -trunkLength/2; 0 0 trunkLength/2],pointSpacing);

N_branch_dir=[0 0 1]*R;
t=linspace(0,2*pi,ceil((2*pi*trunkRadiusOuter)./pointSpacing)+1)'; t=t(1:end-1);
Vc1=trunkRadiusOuter.*[cos(t) sin(t) zeros(size(t))];

Vc2_trunk=Vc1+Vm(end,:);
Vc1_trunk=Vc1+Vm(1,:);

%%

n1=vecnormalize(Vm(2,:)-Vm(1,:));
n2=vecnormalize(Vm(end,:)-Vm(end-1,:));
[Ft,Vt,Ct]=sweepLoft(Vc1_trunk,Vc2_trunk,n1,n2,Vm,[],0,0);

%% Ray trace branch direction onto trunk to get branch centre point

optionStruct.tolEps        = 1e-6;
optionStruct.triSide       = -1;
optionStruct.rayType       = 'ray';
optionStruct.exclusionType = 'inclusive';
optionStruct.paired        = 0;
P=triSurfRayTrace(V_branch_origin,N_branch_dir,Ft,Vt,optionStruct);
P=mean(P,1);

%%
% Visualization250

cFigure; hold on;
title('Main trunk');
hp1=gpatch(Ft,Vt,'w','k',faceAlpha);
hp2=quiverVec(V_branch_origin,N_branch_dir,branchLength*2,'r');
hp3=plotV(P,'b.','markerSize',markerSize1);
legend([hp1 hp2 hp3],{'Main trunk','Branch direction vector','Intersection point'})
axisGeom(gca,fontSize); camlight headlight;
gdrawnow;

%% Cut main trunk using side branch ellipse

t=linspace(0,2*pi,2*ceil((2*pi*branchRadiusOuter)./pointSpacing)+1)'; t=t(1:end-1);
b=branchRadiusOuter./cosd(90-branchAngle);
V_cut_ellipse=[b*cos(t) branchRadiusOuter.*sin(t) zeros(size(t))];
V_cut_ellipse=V_cut_ellipse*R_90+P;
[~,indRemove]=minDist(V_cut_ellipse,Vt);
indRemove=unique(indRemove);
logicFacesSelect=~any(ismember(Ft,indRemove),2);

Ft_precut=Ft(logicFacesSelect,:);

clear optionStruct
optionStruct.outputType='label';
[G,~,groupSize]=tesgroup(Ft_precut,optionStruct);
[~,indLargestGroup]=max(groupSize);
Ft_cut=Ft_precut(G==indLargestGroup,:);

Et_boundary_cut=patchBoundary(Ft_cut);
Et_boundary=patchBoundary(Ft);
Et_cut_boundary=Et_boundary_cut(~any(ismember(Et_boundary_cut,Et_boundary),2),:);
indCutCurve=edgeListToCurve(Et_cut_boundary);

clear optionStruct
optionStruct.numSeeds=numel(indCutCurve); %Number of seeds
optionStruct.waitBarOn=0; %Turn on/off waitbar
Dt_cut=meshDistMarch(Ft_cut,Vt,indCutCurve,optionStruct);
logicVerticesFar=Dt_cut>cutDist;
logicKeep=all(logicVerticesFar(Ft_cut),2);
Ft_cut=Ft_cut(logicKeep,:);

Et_boundary_cut=patchBoundary(Ft_cut);
Et_boundary=patchBoundary(Ft);
Et_cut_boundary=Et_boundary_cut(~any(ismember(Et_boundary_cut,Et_boundary),2),:);
indCutCurve=edgeListToCurve(Et_cut_boundary);

V_branch_curve_trunk=Vt(indCutCurve(1:end-1),:);
[~,~,Nt]=patchNormal(Ft_cut,Vt);

N_branch_curve_trunk=Nt(indCutCurve(1:end-1),:);

%%
% Visualization

cFigure; hold on;
title('Cut main trunk');
hp1=gpatch(Ft_cut,Vt,Dt_cut,'k',faceAlpha);
hp2=quiverVec(V_branch_origin,N_branch_dir,branchLength*2,'r');
hp3=plotV(V_cut_ellipse,'g.-','markerSize',markerSize2,'LineWidth',lineWidth1);
hp4=plotV(V_branch_curve_trunk,'b.-','markerSize',markerSize2,'LineWidth',lineWidth1);
legend([hp1 hp2 hp3 hp4],{'Cut main trunk','Branch direction vector','Branch base ellipse','Cut boundary'})
axisGeom(gca,fontSize); camlight headlight;
gdrawnow;

%% Define bezier start and end vector directions

[~,indNearest]=minDist(V_branch_curve_trunk(1,:),V_cut_ellipse);
dt=linspace(2*pi,0,size(V_branch_curve_trunk,1)+1)'; dt=dt(1:end-1);
dt=dt+t(indNearest);
V_cut_ellipse=[b*cos(dt) branchRadiusOuter.*sin(dt) zeros(size(dt))];
V_branch_curve_ellipse1=V_cut_ellipse*R_90+P+N_branch_dir.*(branchBaseOffset/cosd(90-branchAngle));

d1=sum(sqrt(sum((V_branch_curve_ellipse1-V_branch_curve_trunk).^2,2)));
d2=sum(sqrt(sum((flipud(V_branch_curve_ellipse1)-V_branch_curve_trunk).^2,2)));
if d2<d1
    V_branch_curve_ellipse1=flipud(V_branch_curve_ellipse1);
    dt=flipud(dt);
end

C1=vecnormalize(V_branch_curve_trunk-V_branch_curve_ellipse1);
D1=vecnormalize(cross(C1,N_branch_curve_trunk));
M1=vecnormalize(cross(D1,N_branch_curve_trunk));

M2=N_branch_dir(ones(size(M1,1),1),:);

%%
% Visualization

cFigure; hold on;
title('Cut main trunk');
hp1=gpatch(Ft_cut,Vt,'w','k',1);
hp2=plotV(V_branch_curve_trunk,'r.-','markerSize',markerSize2,'LineWidth',lineWidth1);
hp3=plotV(V_branch_curve_ellipse1,'g.-','markerSize',markerSize2,'LineWidth',lineWidth1);
hp4=quiverVec(V_branch_curve_trunk,M1,branchRadiusOuter/2,'r');
hp5=quiverVec(V_branch_curve_ellipse1,M2,branchRadiusOuter/2,'g');
legend([hp1 hp2 hp3 hp4 hp5],{'Cut main trunk','Cut boundary','Branch base ellipse','Branch base dir.','Branch vessel dir.'})
axisGeom(gca,fontSize); camlight headlight;
gdrawnow;

%% Create raw bezier surface

[Fb1,Vb1,X,Y,Z]=bezierLoft(V_branch_curve_trunk,V_branch_curve_ellipse1,M1,M2,pointSpacing,bezierTangency);

%%
% Visualization

cFigure; hold on;
title('Cut main trunk');
hp1=gpatch(Ft_cut,Vt,'w','k',1);
hp2=plotV(V_branch_curve_trunk,'r.-','markerSize',markerSize2,'LineWidth',lineWidth1);
hp3=plotV(V_branch_curve_ellipse1,'g.-','markerSize',markerSize2,'LineWidth',lineWidth1);
hp4=gpatch(Fb1,Vb1,'bw','k',1,0.5);

for q=1:size(X,2)
    hp5=plotV([X(:,q) Y(:,q) Z(:,q)],'k-','LineWidth',4);
end

legend([hp1 hp2 hp3 hp4 hp5],{'Cut main trunk','Cut boundary','Branch base ellipse','Branch base mesh','Bezier curves'})
axisGeom(gca,fontSize); camlight headlight;
gdrawnow;

%% Loft and merge side branch

endType=1;
switch endType
    case 1 %Circle
        V_branch_curve_ellipse2=branchRadiusOuter.*[cos(dt) sin(dt) zeros(size(dt))];
        V_branch_curve_ellipse2=V_branch_curve_ellipse2*R+P+N_branch_dir.*branchLength;
    case 2 %Ellipse
        V_branch_curve_ellipse2=V_branch_curve_ellipse1+N_branch_dir.*(branchLength-pointSpacing);
end

clear optionStruct
optionStruct.closeLoopOpt=1;
optionStruct.patchType='quad';
[Fb2,Vb2]=polyLoftLinear(V_branch_curve_ellipse1,V_branch_curve_ellipse2,optionStruct);
Fb2=fliplr(Fb2);

[F,V,C]=joinElementSets({Ft_cut,Fb1,Fb2},{Vt,Vb1,Vb2});
[F,V]=mergeVertices(F,V);
[F,V]=patchCleanUnused(F,V);

%% Constrained smoothing of branch base region

indSmoothRegion=F(C==2,:);
for q=1:1:smoothIncludeSteps
    indSmooth=F(any(ismember(F,indSmoothRegion),2),:);
    indSmoothRegion=indSmooth;
end
indRigid=F(~ismember(F,indSmooth));

clear optionStruct
optionStruct.Method=smoothMethod;
optionStruct.n=numSmootIterations;
optionStruct.RigidConstraints=indRigid;
V=patchSmooth(F,V,[],optionStruct);

%%
% Visualization

cFigure; hold on;
title('Merged surface mesh')
hp1=gpatch(F,V,C,'k',1,1);
hp2=patchNormPlot(F,V);
axisGeom(gca,fontSize); camlight headlight;
colormap(gjet); icolorbar;
legend([hp1 hp2],{'Bifurcation mesh','Normal directions'})
gdrawnow;

%% Thicken surface mesh to hexahedra

[E,V]=patchThick(F,V,-1,wallThickness,numElementsWall);
CE=repmat(C,numElementsWall,1);
[FE,CF_E,CF_type]=element2patch(E,CE);

%%
% Visualization

cFigure; hold on;
title('Hexahedral mesh')
gpatch(FE,V,'w','k');
axisGeom(gca,fontSize); camlight headlight;
gdrawnow;

%%

function [F,V,X,Y,Z]=bezierLoft(P1,P4,N1,N4,pointSpacing,f)

D12=sqrt(sum((P1-P4).^2,2));
numPoints=ceil(max(D12)./pointSpacing);
if numPoints<2
    numPoints=2;
end

P2=P1+D12.*f.*N1;
P3=P4-D12.*f.*N4;

X=zeros(numPoints,size(P1,1));
Y=zeros(numPoints,size(P1,1));
Z=zeros(numPoints,size(P1,1));
for q=1:1:size(P1,1)
    p=[P1(q,:); P2(q,:); P3(q,:); P4(q,:)]; %Control points
    V_bezier=bezierCurve(p,numPoints*2); %Compute bezier curve
    V_bezier=evenlySampleCurve(V_bezier,numPoints,'pchip'); %resample evenly
    X(:,q)=V_bezier(:,1);
    Y(:,q)=V_bezier(:,2);
    Z(:,q)=V_bezier(:,3);
end

%Create quad patch data
[F,V] = surf2patch(X,Y,Z);
I=[(1:size(Z,1)-1)' (1:size(Z,1)-1)' (2:size(Z,1))' (2:size(Z,1))' ];
J=[size(Z,2).*ones(size(Z,1)-1,1) ones(size(Z,1)-1,1) ones(size(Z,1)-1,1) size(Z,2).*ones(size(Z,1)-1,1)];
F_sub=sub2ind(size(Z),I,J);
F=[F;F_sub];
F=fliplr(F);

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


% spline 举例
clear; clc;

xi=[27.7, 28, 29, 30]; % 插值节点
yi=[4.1, 4.3, 4.1, 3.0]; % 节点处的函数值
df0=3.0; dfn=-4.0; % 边界条件
pp=spline(xi,[df0, yi, dfn]);

xh=27.7:0.1:30; % 需要插值的点
yh=ppval(pp,xh); % 通过插值求得的近似值
plot(xi,yi,'r+',xh,yh,'o-b','LineWidth',1.5,'MarkerSize',12);


function K = covMaternard(d, hyp, x, z, i)

% Matern covariance function with nu = d/2 and Automatic Relevance Detemination
% (ARD) distance measure. For % d=1 the function is also known as the exponential
% covariance function or the Ornstein-Uhlenbeck covariance. The covariance
% function is:
%
%   k(x^p,x^q) = s2f * f( sqrt(d)*r ) * exp(-sqrt(d)*r)
%
% with f(t)=1 for d=1, f(t)=1+t for d=3 and f(t)=1+t.*(1+t/3) for d=5.
% Here r is the distance sqrt((x^p-x^q)'*inv(P)*(x^p-x^q)), P is ell times
% the unit matrix and sf2 is the signal variance. The hyperparameters are:
%
% hyp = [ log(ell_1)
%         log(ell_2)
%          .
%         log(ell_D)
%         log(sqrt(sf2)) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%                  Ali Abusnina, 2014
%
% See also COVFUNCTIONS.M.

if nargin<3, K = '(D+1)'; return; end              % report number of parameters
if nargin<4, z = []; end                                   % make sure, z exists
xeqz = numel(z)==0; dg = strcmp(z,'diag') && numel(z)>0;        % determine mode

[n,D] = size(x);
ell = exp(hyp(1:D));                               % characteristic length scale
sf2 = exp(2*hyp(D+1));

switch d
  case 1, f = @(t) 1;               df = @(t) 1;
  case 3, f = @(t) 1 + t;           df = @(t) t;
  case 5, f = @(t) 1 + t.*(1+t/3);  df = @(t) t.*(1+t)/3;
end
          m = @(t,f) f(t).*exp(-t); dm = @(t,f) df(t).*t.*exp(-t);

% precompute distances
if dg                                                               % vector kxx
  K = zeros(size(x,1),1);
else
  if xeqz                                                 % symmetric matrix Kxx
      K = sqrt( sq_dist(sqrt(d)* (bsxfun(@rdivide, x', ell))    ));

  else                                                   % cross covariances Kxz

    K = sqrt( sq_dist(sqrt(d)* (bsxfun(@rdivide, x', ell)) , sqrt(d) * (bsxfun(@rdivide, z', ell))) );

  end
end

if nargin<5                                                        % covariances
  K = sf2*m(K,f);
else                                                               % derivatives
  if i<=D
    K = sf2*dm(K,f);
  elseif i<=D+1
    K = 2*sf2*m(K,f);
  else
    error('Unknown hyperparameter')
  end
end


classdef MatrixVectorizedInverter_3x3 < MatrixVectorizedInverter_Interface

    methods (Access = public)

        function B = computeInverse(obj,A)
            d = obj.computeAdjointsTranspose(A);

            det = obj.computeDeterminant(A);

            B = zeros(size(A));
            for i = 1:3
                for j = 1:3
                    B(i,j,:) = squeeze(d(i,j,:))./det;
                end
            end
        end

        function detA = computeDeterminant(obj,A)
            d    = obj.computeAdjointsTranspose(A);
            d = permute(d,[2 1 3]);
            detA = squeeze(sum(A.*d,[1 2]))/3;
        end

    end

    methods (Access = private, Static)

        function d = computeAdjointsTranspose(A)
            d = zeros(size(A));

            A11 = A(1,1,:);
            A12 = A(1,2,:);
            A13 = A(1,3,:);
            A21 = A(2,1,:);
            A22 = A(2,2,:);
            A23 = A(2,3,:);
            A31 = A(3,1,:);
            A32 = A(3,2,:);
            A33 = A(3,3,:);

            d(1,1,:) = A22.*A33-A23.*A32;
            d(1,2,:) = A32.*A13-A33.*A12;
            d(1,3,:) = A12.*A23-A13.*A22;

            d(2,1,:) = A23.*A31-A21.*A33;
            d(2,2,:) = A33.*A11-A31.*A13;
            d(2,3,:) = A13.*A21-A11.*A23;

            d(3,1,:) = A21.*A32-A22.*A31;
            d(3,2,:) = A31.*A12-A32.*A11;
            d(3,3,:) = A11.*A22-A12.*A21;
        end

    end

end


function value = epn_lag_monomial_integral ( n, expon )

%*****************************************************************************80
%
%% EPN_LAG_MONOMIAL_INTEGRAL: integral of monomial with Laguerre weight on EPN.
%
%  Discussion:
%
%    EPN is the N-dimensional positive space [0,+oo)^N with exponential
%    or Laguerre weight function:
%
%      w(x(1:n)) = exp ( - sum ( x(1:n) ) )
%
%    value = integral ( EPN )
%      product ( 1 <= i <= n ) x(I)^expon(i) exp ( -x(i) ) dx(i)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, integer EXPON(N), the exponents.
%
%    Output, real VALUE, the value of the integral.
%
  value = 1.0;
  for i = 1 : n
    value2 = ep1_lag_monomial_integral ( expon(i) );
    value = value * value2;
  end

  return
end


function value = epn_glg_monomial_integral ( n, expon, alpha )

%*****************************************************************************80
%
%% EPN_GLG_MONOMIAL_INTEGRAL: integral of monomial with GLG weight on EPN.
%
%  Discussion:
%
%    EPN_GLG is the N-dimensional positive space [0,+oo)^N with generalized
%    Laguerre weight function:
%
%      w(alpha;x) = product ( 1 <= i <= n ) x(i)^alpha exp ( - x(i) )
%
%    value = integral ( EPN )
%      product ( 1 <= i <= n ) x(I)^expon(i) x(i)^alpha exp ( - x(i) ) dx(i)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, integer EXPON(N), the exponents.
%
%    Input, real ALPHA, the exponent of X in the weight function.
%    -1.0 < ALPHA.
%
%    Output, real VALUE, the value of the integral.
%
  value = 1.0;
  for i = 1 : n
    value2 = ep1_glg_monomial_integral ( expon(i), alpha );
    value = value * value2;
  end

  return
end


function value = r8_binom ( n, m )

%*****************************************************************************80
%
%% R8_BINOM evaluates the binomial coefficient using R8 arithmetic.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, integer N, M, the arguments.
%
%    Output, real VALUE, the binomial coefficient.
%
  persistent bilnmx
  persistent fintmx
  persistent sq2pil

  sq2pil = 0.91893853320467274178032973640562;

  if ( isempty ( bilnmx ) )
    bilnmx = log ( r8_mach ( 2 ) ) - 0.0001;
    fintmx = 0.9 / r8_mach ( 3 );
  end

  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BINOM - Fatal error!\n' );
    fprintf ( 1, '  N < 0.\n' );
    error ( 'R8_BINOM - Fatal error!' )
  end

  if ( m < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BINOM - Fatal error!\n' );
    fprintf ( 1, '  M < 0.\n' );
    error ( 'R8_BINOM - Fatal error!' )
  end

  if ( n < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BINOM - Fatal error!\n' );
    fprintf ( 1, '  N < M.\n' );
    error ( 'R8_BINOM - Fatal error!' )
  end

  k = min ( m, n - m );

  if ( k <= 20 && k * log ( max ( n, 1 ) ) <= bilnmx )

    value = 1.0;

    for i = 1 : k
      value = value * ( n - i + 1 ) / i;
    end

  else

    if ( k < 9 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8_BINOM - Fatal error!\n' );
      fprintf ( 1, '  Result overflows.\n' );
      fprintf ( 1, '  N or M is too big.\n' );
      error ( 'R8_BINOM - Fatal error!' )
    end

    xn =  n + 1;
    xk =  k + 1;
    xnk =  n - k + 1;

    corr = r8_lgmc ( xn ) - r8_lgmc ( xk ) - r8_lgmc ( xnk );

    value = xk * log ( xnk / xk ) ...
      - xn * r8_lnrel ( - ( xk - 1.0 ) / xn ) ...
      - 0.5 * log ( xn * xnk / xk ) + 1.0 - sq2pil + corr;

    if ( bilnmx < value )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8_BINOM - Fatal error!\n' );
      fprintf ( 1, '  Result overflows.\n' );
      fprintf ( 1, '  N or M is too big.\n' );
      error ( 'R8_BINOM - Fatal error!' )
    end

    value = exp ( value );

  end

  if ( value < fintmx )
    value = r8_aint ( value + 0.5 );
  end

  return
end


% Jason Joseph Rebello
% Carnegie Mellon University (Jan 2012 - May 2013)
% MS in Electrical & Computer Engineering
% K Means Algorithm with the application to image compression

% This program uses the K means clustering algorithm to group the pixels
% in an image in order to provide image compression.

clear all;
clc;
close all;

fprintf('K means clustering algorithm used for image compression\n\n');
t = cputime;

%% Read the image
fprintf('Reading image');
I = imread('bird_small.png');
%imshow(I);
I = (double(I))/255;
fprintf('...done\n\n');

%% Declare and Initialize Variabels
fprintf('Initializing variables');
K = 16; % number of clusters
imgSize = size(I); % get size of image
iterCentroids = 10; % number of times K means runs to find the best centroid
iterKMeans = 10; % number of times K means runs with different initial centroids
fprintf('...done\n\n');

%% Get input
fprintf('Formatting input');
X = reshape(I, imgSize(1) * imgSize(2), 3); % resize into (total pixel x features)
fprintf('...done\n\n');

%% Run K Means
for i=1:iterKMeans

    fprintf(' ********* Running K means iteration %d ***********\n\n',i);
    [centroids cost idx] = runKMeans(X, K, iterCentroids);
    fprintf('Cost after %d iteration : %f\n\n',i,cost);

    if i==1
        bestCentroids = centroids;
        bestCost = cost;
        bestidx = idx;
    elseif (i>1 && cost<bestCost) % stores the best clustering
        bestCentroids = centroids;
        bestCost = cost;
        bestidx = idx;
    end
    fprintf('Best cost : %f\n\n',bestCost);

XCompressed = centroids(idx,:);

% Reshape the recovered image into proper dimensions
XCompressed = reshape(XCompressed, imgSize(1), imgSize(2), 3);
imshow(XCompressed); % display final compressed image for each iteration
pause(1);

end

%% Display original and best compressed image
XCompressed = bestCentroids(bestidx,:);

% Reshape the recovered image into proper dimensions
XCompressed = reshape(XCompressed, imgSize(1), imgSize(2), 3);
displayImage(I, XCompressed, K);

fprintf('Program executed in %f seconds or %f minutes\n\n', cputime-t, (cputime-t)/60);


function z=fisherz(r);
% function z=fisherz(r);
%
% transforms r scores (correlations) into fisher z scores by the
% transform r = 0.5*log(1+r)/(1-r);
%
% rmk, 1/14/99

z=0.5*log((1+r)./(1-r));


function [ n_data, x, fx ] = si_values ( n_data )

%*****************************************************************************80
%
%% SI_VALUES returns some values of the sine integral function.
%
%  Discussion:
%
%    SI(X) = integral ( 0 <= T <= X ) sin ( T ) / T dt
%
%    In Mathematica, the function can be evaluated by:
%
%      SinIntegral[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 16;

  fx_vec = [ ...
     0.4931074180430667E+00, ...
     0.5881288096080801E+00, ...
     0.6812222391166113E+00, ...
     0.7720957854819966E+00, ...
     0.8604707107452929E+00, ...
     0.9460830703671830E+00, ...
     0.1108047199013719E+01, ...
     0.1256226732779218E+01, ...
     0.1389180485870438E+01, ...
     0.1505816780255579E+01, ...
     0.1605412976802695E+01, ...
     0.1778520173443827E+01, ...
     0.1848652527999468E+01, ...
     0.1833125398665997E+01, ...
     0.1758203138949053E+01, ...
     0.1654140414379244E+01 ];

  x_vec = [ ...
      0.5E+00, ...
      0.6E+00, ...
      0.7E+00, ...
      0.8E+00, ...
      0.9E+00, ...
      1.0E+00, ...
      1.2E+00, ...
      1.4E+00, ...
      1.6E+00, ...
      1.8E+00, ...
      2.0E+00, ...
      2.5E+00, ...
      3.0E+00, ...
      3.5E+00, ...
      4.0E+00, ...
      4.5E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function x = chi_cdf_inv ( cdf, a, b, c )

%*****************************************************************************80
%
%% CHI_CDF_INV inverts the Chi CDF.
%
%  Discussion:
%
%    A simple bisection method is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, B, C, the parameters of the PDF.
%    0 < B,
%    0 < C.
%
%    Output, real X, the corresponding argument of the CDF.
%
  it_max = 100;
  tol = 0.0001;

  if ( cdf <= 0.0 )
    x = a;
    return
  elseif ( 1.0 <= cdf )
    x = r8_huge ( );
    return
  end

  x1 = a;
  cdf1 = 0.0;

  x2 = a + 1.0;

  while ( 1 )

    cdf2 = chi_cdf ( x2, a, b, c );

    if ( cdf < cdf2 )
      break;
    end

    x2 = a + 2.0 * ( x2 - a );

  end
%
%  Now use bisection.
%
  it = 0;

  while ( 1 )

    it = it + 1;

    x3 = 0.5 * ( x1 + x2 );
    cdf3 = chi_cdf ( x3, a, b, c );

    if ( abs ( cdf3 - cdf ) < tol )
      x = x3;
      return
    end

    if ( it_max < it )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CHI_CDF_INV - Fatal error!\n' );
      fprintf ( 1, '  Iteration limit exceeded.\n' );
      error ( 'CHI_CDF_INV - Fatal error!' );
    end

    if ( ( cdf3 < cdf & cdf1 < cdf ) | ( cdf < cdf3 & cdf < cdf1 ) )
      x1 = x3;
      cdf1 = cdf3;
    else
      x2 = x3;
      cdf2 = cdf3;
    end

  end

  return
end


%DEMGMM1 Demonstrate EM for Gaussian mixtures.
%
%	Description
%	This script demonstrates the use of the EM algorithm to fit a mixture
%	of Gaussians to a set of data using maximum likelihood. A colour
%	coding scheme is used to illustrate the evaluation of the posterior
%	probabilities in the E-step of the EM algorithm.
%
%	See also
%	DEMGMM2, DEMGMM3, DEMGMM4, GMM, GMMEM, GMMPOST
%

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

clc;
disp('This demonstration illustrates the use of the EM (expectation-')
disp('maximization) algorithm for fitting of a mixture of Gaussians to a')
disp('data set by maximum likelihood.')
disp(' ')
disp('The data set consists of 40 data points in a 2-dimensional')
disp('space, generated by sampling from a mixture of 2 Gaussian')
disp('distributions.')
disp(' ')
disp('Press any key to see a plot of the data.')
pause;

% Generate the data
randn('state', 0); rand('state', 0);
gmix = gmm(2, 2, 'spherical');
ndat1 = 20; ndat2 = 20; ndata = ndat1+ndat2;
gmix.centres =  [0.3 0.3; 0.7 0.7];
gmix.covars = [0.01 0.01];
x = gmmsamp(gmix, ndata);

h = figure;
hd = plot(x(:, 1), x(:, 2), '.g', 'markersize', 30);
hold on; axis([0 1 0 1]); axis square; set(gca, 'box', 'on');
ht = text(0.5, 1.05, 'Data', 'horizontalalignment', 'center');
disp(' ');
disp('Press any key to continue.')
pause; clc;

disp('We next create and initialize a mixture model consisting of a mixture')
disp('of 2 Gaussians having ''spherical'' covariance matrices, using the')
disp('function GMM. The Gaussian components can be displayed on the same')
disp('plot as the data by drawing a contour of constant probability density')
disp('for each component having radius equal to the corresponding standard')
disp('deviation. Component 1 is coloured red and component 2 is coloured')
disp('blue.')
disp(' ')
disp('Note that a particulary poor choice of initial parameters has been')
disp('made in order to illustrate more effectively the operation of the')
disp('EM algorithm.')
disp(' ')
disp('Press any key to see the initial configuration of the mixture model.')
pause;

% Set up mixture model
ncentres = 2; input_dim = 2;
mix = gmm(input_dim, ncentres, 'spherical');

% Initialise the mixture model
mix.centres = [0.2 0.8; 0.8, 0.2];
mix.covars = [0.01 0.01];

% Plot the initial model
ncirc = 30; theta = linspace(0, 2*pi, ncirc);
xs = cos(theta); ys = sin(theta);
xvals = mix.centres(:, 1)*ones(1,ncirc) + sqrt(mix.covars')*xs;
yvals = mix.centres(:, 2)*ones(1,ncirc) + sqrt(mix.covars')*ys;
hc(1)=line(xvals(1,:), yvals(1,:), 'color', 'r');
hc(2)=line(xvals(2,:), yvals(2,:), 'color', 'b');
set(ht, 'string', 'Initial Configuration');
figure(h);
disp(' ')
disp('Press any key to continue');
pause; clc;

disp('Now we adapt the parameters of the mixture model iteratively using the')
disp('EM algorithm. Each cycle of the EM algorithm consists of an E-step')
disp('followed by an M-step.  We start with the E-step, which involves the')
disp('evaluation of the posterior probabilities (responsibilities) which the')
disp('two components have for each of the data points.')
disp(' ')
disp('Since we have labelled the two components using the colours red and')
disp('blue, a convenient way to indicate the value of a posterior')
disp('probability for a given data point is to colour the point using a')
disp('scale ranging from pure red (corresponding to a posterior probability')
disp('of 1.0 for the red component and 0.0 for the blue component) through')
disp('to pure blue.')
disp(' ')
disp('Press any key to see the result of applying the first E-step.')
pause;

% Initial E-step.
set(ht, 'string', 'E-step');
post = gmmpost(mix, x);
dcols = [post(:,1), zeros(ndata, 1), post(:,2)];
delete(hd);
for i = 1 : ndata
  hd(i) = plot(x(i, 1), x(i, 2), 'color', dcols(i,:), ...
          'marker', '.', 'markersize', 30);
end
figure(h);

disp(' ');
disp('Press any key to continue')
pause; clc;

disp('Next we perform the corresponding M-step. This involves replacing the')
disp('centres of the component Gaussians by the corresponding weighted means')
disp('of the data. Thus the centre of the red component is replaced by the')
disp('mean of the data set, in which each data point is weighted according to')
disp('the amount of red ink (corresponding to the responsibility of')
disp('component 1 for explaining that data point). The variances and mixing')
disp('proportions of the two components are similarly re-estimated.')
disp(' ')
disp('Press any key to see the result of applying the first M-step.')
pause;

% M-step.
set(ht, 'string', 'M-step');
options = foptions;
options(14) = 1; % A single iteration
options(1) = -1; % Switch off all messages, including warning
mix = gmmem(mix, x, options);
delete(hc);
xvals = mix.centres(:, 1)*ones(1,ncirc) + sqrt(mix.covars')*xs;
yvals = mix.centres(:, 2)*ones(1,ncirc) + sqrt(mix.covars')*ys;
hc(1)=line(xvals(1,:), yvals(1,:), 'color', 'r');
hc(2)=line(xvals(2,:), yvals(2,:), 'color', 'b');
figure(h);
disp(' ')
disp('Press any key to continue')
pause; clc;

disp('We can continue making alternate E and M steps until the changes in')
disp('the log likelihood at each cycle become sufficiently small.')
disp(' ')
disp('Press any key to see an animation of a further 9 EM cycles.')
pause;
figure(h);

% Loop over EM iterations.
numiters = 9;
for n = 1 : numiters

  set(ht, 'string', 'E-step');
  post = gmmpost(mix, x);
  dcols = [post(:,1), zeros(ndata, 1), post(:,2)];
  delete(hd);
  for i = 1 : ndata
    hd(i) = plot(x(i, 1), x(i, 2), 'color', dcols(i,:), ...
                 'marker', '.', 'markersize', 30);
  end
  pause(1)

  set(ht, 'string', 'M-step');
  [mix, options] = gmmem(mix, x, options);
  fprintf(1, 'Cycle %4d  Error %11.6f\n', n, options(8));
  delete(hc);
  xvals = mix.centres(:, 1)*ones(1,ncirc) + sqrt(mix.covars')*xs;
  yvals = mix.centres(:, 2)*ones(1,ncirc) + sqrt(mix.covars')*ys;
  hc(1)=line(xvals(1,:), yvals(1,:), 'color', 'r');
  hc(2)=line(xvals(2,:), yvals(2,:), 'color', 'b');
  pause(1)

end

disp(' ');
disp('Press any key to end.')
pause; clc; close(h); clear all


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) A simulation of a DC motor.
%
% Help: Function prototype
% [y, t] = runge_kutta(f, y0, [t0 tfinal], timestep)
% where f is the function being integrated as dy/dt = f(t, y).
% y0 are the initial conditions
% t0: initial time
% tfinal: final time.
% h: time step for the calculations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function simulate_motor_runge_kutta()
close all;
t0 = 0;
tfinal = 15;

[t, y] = runge_kutta(@motor, [0 0 0]', [t0 tfinal], 0.01);
y = y(:, 1:length(t));
plot(t, y(1,:), 'r'), hold
plot(t, y(2,:), 'g')
plot(t, y(3,:), 'b')
legend('Position theta (rad)', 'Speed (rad/s)', 'Current (A)')

figure,
plot(y(1,:), y(2,:), 'r')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function for Runge-Kutta.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = motor(t, x)
% We must return the solution of
% [dx1/dt; dx2/dt; dx3/dt dx4/dt]
Jm=500*1e-3; %kg*m2
R=0.566;%(Ohm)
L=0.172e-3;%(H)
Kb=1/(183*(pi/30)); % V/rad/s
Kp= 52.2/1000;
Bm = 0.001;
G=1;
tau = 0; % Nm
V = 5; % Volt

thetadd = (1/Jm)*(-Bm*x(2)+Kp*x(3)-tau/G);
currentd = (1/L)*(V-Kb*x(2)-R*x(3));

xd(1) = x(2);
xd(2) = thetadd;
xd(3) = currentd;
xd = xd(:);


function [r,x,y,z] = elec_sphere_project(X,Y,Z,xo,yo,zo,varargin)

% elec_sphere_project - calculates spherical projections of electrode positions
%
% Usage: [r,x,y,z] = elec_sphere_project(X,Y,Z,xo,yo,zo,[estim],[plot])
%
% Notes:    The general formula for a sphere, with radius r is given by:
%
%           (x - xo)^2  +  (y - yo)^2  +  (z - zo)^2  =  r^2
%
%           This function takes arguments for cartesian co-ordinates
%           of X,Y,Z (assume Z > 0) and the center of the sphere (xo,yo,zo).
%           If (xo,yo,zo) are not provided, they are assumed (0,0,0).
%
%           Returned values are the fitted radius 'r' (constant)
%           and the (x,y,z) Cartesian co-ordinates on the projected sphere.
%
%           Options: estim - echo spherical radius estimates (default),
%                    plot  - plot the input/projected xyz on a sphere.
%

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

% Licence:  GNU GPL, no express or implied warranties
% History:  02/2002, Darren.Weber_at_radiology.ucsf.edu
%                    adapted from elec_fit_sph
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% initialise centroid, unless input parameters defined
if ~exist('xo','var') xo = 0; else, if isempty(xo) xo = 0; end, end
if ~exist('yo','var') yo = 0; else, if isempty(yo) yo = 0; end, end
if ~exist('zo','var') zo = 0; else, if isempty(zo) zo = 0; end, end

if (nargin>6),        estim = varargin{1};
else                  estim = 1;
end
if (nargin>7),        plot = varargin{2};
else                  plot = 0;
end

eegversion = '$Revision: 1.1 $';
fprintf('ELEC_SPHERE_PROJECT [v %s]\n',eegversion(11:15));
fprintf('...projecting electrodes to spherical points\n');
tic;

% Initialise r0 as a rough guess at the sphere radius
rX = (max(X) - min(X)) / 2;
rY = (max(Y) - min(Y)) / 2;
rZ =  max(Z) - zo;
r0 = mean([ rX rY rZ ]);

if isequal(estim,1),
    fprintf('...initial spherical radius   = %f\n', r0); end

% perform least squares estimate of spherical radius (r)
options = optimset('fminsearch');
[r,fval,exitflag,output] = fminsearch('elec_sphere_fit_optim',...
                           r0, options, X, Y, Z, xo, yo, zo);
%fprintf('\n%s%f\n', 'Iterations = ', output.iterations);
%fprintf('\n%s%d\n', 'Exit = ', exitflag);

if isequal(estim,1),
    fprintf('...estimated spherical radius = %f\n', r); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the projection point of X,Y,Z to the fitted sphere radius r

% Convert Cartesian X,Y,Z to spherical (radians)
theta = atan2( (Y-yo), (X-xo) );
phi = atan2( sqrt( (X-xo).^2 + (Y-yo).^2 ), (Z-zo) );
% do not recalc: r = sqrt( (X-xo).^2 + (Y-yo).^2 + (Z-zo).^2);

%   Recalculate X,Y,Z for constant r, given theta & phi.
R = ones(size(phi)) * r;
x = R .* sin(phi) .* cos(theta);
y = R .* sin(phi) .* sin(theta);
z = R .* cos(phi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the input & projected electrode positions on a sphere
if isequal(plot,1),
    figure('NumberTitle','off','Name','Electrode Placements');
    set(gca,'Projection','perspective');
    set(gca,'DataAspectRatio',[1 1 1]);

    hold on

    plot3(x,y,z,'b.');
    plot3(X,Y,Z,'ro');
    legend('input xyz','fitted sphere','Location','BestOutside');

    [Xs,Ys,Zs]=sphere;
    Xs = Xs * r;
    Ys = Ys * r;
    Zs = Zs * r;

    surf(Xs,Ys,Zs,...
        'EdgeColor','none',...
        'FaceColor',[0.7 0.7 0.7],...
        'FaceAlpha',0.4);
    view(2);
    rotate3d;

    axis tight; hold off;
end

t = toc; fprintf('...done (%6.2f sec).\n\n',t);

return


% Calculate statistics of a rate map that depend on probability distribution function (PDF)
%
% Calculates information, sparsity and selectivity of a rate map. Calculations are done
% according to 1993 Skaggs et al. "An Information-Theoretic Approach to Deciphering the Hippocampal Code"
% paper. Another source of information is 1996 Skaggs et al. paper called
% "Theta phase precession in hippocampal neuronal populations and the compression of temporal sequences".
%
%  USAGE
%   [information, sparsity, selectivity] = analyses.mapStatsPDF(map)
%   map             Structure with rate map, output of analyses.map
%   information     Structure with fields:
%       rate        information rate [bits/sec]
%       content     Spatial information content [bits/spike]
%   sparsity        Sparsity value
%   selectivity     Selectivity value
%
%  SEE
%   See also analyses.map
%
function [information, sparsity, selectivity] = mapStatsPDF(map)
    if ~isstruct(map)
        error('BNT:arg', 'Incorrect argument. Map should be a structure. You are probably relying on old code when mapStatsPDF accepted map as matrix.');
    end
    T = nansum(nansum(map.time)); % overall trial duration
    posPDF = map.time / (T+eps); % probability of animal being in bin x:
                                 % duration of time animal spent in the bin divided by overall trial duration.

    meanrate = nansum(nansum(map.z .* posPDF));
    meansquarerate = nansum(nansum( (map.z .^ 2) .* posPDF ));
    if meansquarerate == 0
       sparsity = NaN;
    else
        sparsity = meanrate^2 / meansquarerate;
    end

    maxrate = nanmax(nanmax(map.z));
    if meanrate == 0;
       selectivity = NaN;
       information.content = nan;
       information.rate = nan;
    else
       selectivity = maxrate / meanrate;
       logArg = map.z / meanrate;
       logArg(logArg < 1) = 1;

       information.rate = nansum(nansum(posPDF .* map.z .* log2(logArg)));
       information.content = information.rate / meanrate;
    end
end


function gauss = get_gaussian_kernel(Nsigmas, sigma_in_pix)
% generate gaussian kernel with given sigmas
    ndims = numel(sigma_in_pix);
    width = 2 * floor(Nsigmas .* sigma_in_pix) + 1;
    hw = (width - 1)/2;

    sigma_in_pix(sigma_in_pix == 0) = 1;
    if ndims == 3
        -hw(1):hw(1)
        hw(2)
        hw(3)
        [n1, n2, n3] = ndgrid(-hw(1):hw(1), -hw(2):hw(2), -hw(3):hw(3));
        size(n1)
        weight = exp(-n1.^2/(2*sigma_in_pix(1)^2) - n2.^2/(2*sigma_in_pix(2)^2) - n3.^2/(2*sigma_in_pix(3)^2) );
    elseif ndims == 2
        [n1, n2] = ndgrid(-hw(1):hw(1), -hw(2):hw(2));
        weight = exp(-n1.^2/(2*sigma_in_pix(1)^2) - n2.^2/(2*sigma_in_pix(2)^2));
    end

    gauss = weight / sum(weight(:));
end


% Fig. 6.29   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=[1 1];
den=conv([1 0],[0.1 -1]);
rlocus(num,den);

axis([-8 12 -10 10]);
axis equal
grid;
title('Fig. 6.29 Root locus for Example 6.10');


function [A_K_I,A_K_F,A_F_I,A_Rfl_F,A_Rfr_F,A_down_up] = Rotation_Matrices(phi,chi,delta)
%% Rotation from rear axle fixed coordinate system into vehicle fixed coordinate system
% Input parameters:
% phi       [rad]    Vehicle Rotation Angles [Phi Theta Psi] [3x1]
% chi       [rad]    Road Cardan Angles [chi_x chi_y Psi] [3x1]
% delta     [rad]    Steering Angles [deltaFL deltaFR 0 0] [1x4]
% Output parameters:
% A_K_I     [---]    Transformation Matrix A_K_I [3x3]
% A_K_F     [---]    Transformation Matrix A_K_F [3x3]
% A_F_I     [---]    Transformation Matrix A_F_I [3x3]
% A_Rfl_F   [---]    Transformation Matrix A_Rfl_F [3x3]
% A_Rfr_F   [---]    Transformation Matrix A_Rfr_F [3x3]
% A_down_up [---]    Transformation Matrix W := A_down_up [3x3]

%% Pre-Computation of trigonometric values
cphi=cos(phi);
sphi=sin(phi);
cchi=cos(chi);
schi=sin(chi);

%% Definition of rotation matrices

% rotation matrix from z-up coordinate system to z-down coordinate system
A_down_up = reshape([1 0 0,0 -1 0,0 0 -1],[3,3]);

% rotation matrix from inertial coordinate system to vehicle fixed coordinate system
A_K_I     = reshape([cphi(3)*cphi(2),-cphi(1)*sphi(3)+cphi(3)*sphi(2)*sphi(1),sphi(3)*sphi(1)+cphi(3)*cphi(1)*sphi(2),cphi(2)*sphi(3),cphi(3)*cphi(1)+sphi(3)*sphi(2)*sphi(1),-cphi(3)*sphi(1)+cphi(1)*sphi(3)*sphi(2),-sphi(2),cphi(2)*sphi(1),cphi(2)*cphi(1)],[3,3]);

% rotation matrix from rear axle fixed coordinate system to vehicle fixed coordinate system without road inclination
% A_K_F     = reshape([cphi(2),sphi(2)*sphi(1),cphi(1)*sphi(2),0,cphi(1),-sphi(1),-sphi(2),cphi(2)*sphi(1),cphi(2)*cphi(1)],[3,3]);

% rotation matrix from rear axle fixed coordinate system to vehicle fixed coordinate system over inertial coordinate system with road inclination
A_K_F     = reshape([cos(phi(2)-chi(2)),sin(phi(2)-chi(2))*sphi(1),sin(phi(2)-chi(2))*cphi(1),-sin(-chi(2)+phi(2))*schi(1),cchi(1)*cphi(1)+cphi(2)*cchi(2)*schi(1)*sphi(1)+sphi(2)*schi(1)*schi(2)*sphi(1),-cchi(1)*sphi(1)+cphi(2)*cchi(2)*cphi(1)*schi(1)+cphi(1)*sphi(2)*schi(1)*schi(2),-sin(-chi(2)+phi(2))*cchi(1),-cphi(1)*schi(1)+cphi(2)*cchi(1)*cchi(2)*sphi(1)+cchi(1)*sphi(2)*schi(2)*sphi(1),schi(1)*sphi(1)+cphi(2)*cchi(1)*cchi(2)*cphi(1)+cchi(1)*cphi(1)*sphi(2)*schi(2)],[3,3]);

% rotation matrix from inertial coordinate system to rear axle fixed coordinate system
A_F_I     = reshape([cphi(3)*cchi(2),-cchi(1)*sphi(3)+cphi(3)*schi(1)*schi(2),sphi(3)*schi(1)+cphi(3)*cchi(1)*schi(2),cchi(2)*sphi(3),cphi(3)*cchi(1)+sphi(3)*schi(1)*schi(2),-cphi(3)*schi(1)+cchi(1)*sphi(3)*schi(2),-schi(2),cchi(2)*schi(1),cchi(1)*cchi(2)],[3,3]);

% rotation matrix from rear axle fixed coordinate system to wheel fixed coordinate systems of front axle
A_Rfl_F   = reshape([cos(delta(1)),-sin(delta(1)),0,sin(delta(1)),cos(delta(1)),0,0,0,1],[3,3]);
A_Rfr_F   = reshape([cos(delta(2)),-sin(delta(2)),0,sin(delta(2)),cos(delta(2)),0,0,0,1],[3,3]);

end


function [sigma,bnd] = bdsqr(alpha,beta)

% BDSQR: Compute the singular values and bottom element of
%        the left singular vectors of a (k+1) x k lower bidiagonal
%        matrix with diagonal alpha(1:k) and lower bidiagonal beta(1:k),
%        where length(alpha) = length(beta) = k.
%
% [sigma,bnd] = bdsqr(alpha,beta)
%
% Input parameters:
%   alpha(1:k)   : Diagonal elements.
%   beta(1:k)    : Sub-diagonal elements.
% Output parameters:
%   sigma(1:k)  : Computed eigenvalues.
%   bnd(1:k)    : Bottom elements in left singular vectors.

% Below is a very slow replacement for the BDSQR MEX-file.

% warning('PROPACK:NotUsingMex','Using slow matlab code for bdsqr.')
k = length(alpha);
if min(size(alpha)') ~= 1  | min(size(beta)') ~= 1
  error('alpha and beta must be vectors')
elseif length(beta) ~= k
  error('alpha and beta must have the same lenght')
end
B = spdiags([alpha(:),beta(:)],[0,-1],k+1,k);
[U,S,V] = svd(full(B),0);
sigma = diag(S);
bnd = U(end,1:k)';


function [B,EV,ED] = biharmonic_embedding(V,F,dim,p);
  % [B,EV,ED] = biharmonic_embedding(V,F);
  %
  % Takes a mesh (V,F) and returns an embedding using the spectrum of the
  % biharmonic operator. Then the biharmonic distance between two points i and
  % j can be computed as the euclidean distance between B(i,:) and B(j,:),
  % namely:
  %   dist_ij = sqrt(sum((B(i,:)-B(j,:)).^2,2));
  %
  % Input:
  %   V  vertex list
  %   F  face list
  %   dim requested dimension of the embedding
  %   Optional:
  %     p  exponent above eigen values
  %       0.5  "semi-harmonic" embedding
  %       1  commute time embedding, "harmonic"
  %       2  biharmonic {default}
  %       3  "triharmonic" embedding
  % Output:
  %   B  biharmonic embedding
  %   EV  eigenvectors used in embedding
  %   ED  eigenvalues used in embedding
  %

  % if dimension is not specfied use 4
  if(~exist('dim','var'))
    dim = 4;
  end

  if(~exist('p','var'))
    p = 2;
  end

  % get cotangent matrix
  L = cotmatrix(V,F);
  % This should be better, but yaron seemed to use barycentric
  %M = massmatrix(V,F);
  M = massmatrix(V,F,'barycentric');

  % get dim+1 smallest magnitude eigenvalues and corresponding vectors
  [EV,ED] = eigs(L,M,dim+1,'sm');
  EV = EV(:, 2:end);
  ED = ED(2:end, 2:end);

  % This is not exactly the same, essentially it removes the mass matrix and
  % multiplies everything by a factos of -2
  % % This also works, because of the sign change in the eigenvalues matlab
  % % reverses the output order so 0.0 is the last eigenvalue
  % [EV,ED] = eigs(-2*L,M./sum(M(:)),dim+1,'sm');
  % EV = EV(:, 1:end-1);
  % ED = ED(1:end-1, 1:end-1);

  %  divide each eigenvector by corresponding eigenvalue
  %  divide the power by 2 first because it will appear in the denominator of
  %  distance computation *outside* the squared difference see (4) and (11)
  B = EV * (inv(abs(ED))^(p/2));
end


 function f = nufft_diric(k, N, K, use_true_diric)
%function f = nufft_diric(k, N, K, use_true_diric)
% "regular fourier" Dirichlet-function WITHOUT phase
% diric(t) = sin(pi N t / K) / ( N * sin(pi t / K) )
%	\approx sinc(t / (K/N))
% in:
%	k [...]		sample locations (unitless real numbers)
%	N		signal length
%	K		DFT length
%	use_true_diric	1 = use true Diric function.
%			(default is to use sinc approximation)
% out:
%	f [...]		corresponding function values
%
% Copyright 2001-12-8, Jeff Fessler, The University of Michigan

% default is to plot
if nargin < 3
	help(mfilename)
	kmax = 2 * (10 + 1 * 4);
	k = linspace(-kmax,kmax,201);
	ki = [-kmax:kmax];
	N = 32;
	K = 2*N;
	g = nufft_diric(k, N, K, 1);
	gi = nufft_diric(ki, N, K, 1);
	s = nufft_diric(k, N, K);
	dm = diric((2*pi/K)*k,N);
	plot(k, g, 'y-', k, s, 'c-', k, dm, 'r-', ki, gi, '.'), axis tight
	legend('nufft diric', 'sinc', 'matlab diric')
	xlabel k, ylabel diric(k)
	printf('max %% difference = %g', max_percent_diff(g,s))
	return
end

if nargin < 4
	use_true_diric = logical(0);
end

% diric version
if use_true_diric
	t = (pi/K) * k;
	f = sin(t);
	i = abs(t) > 1e-12;	% nonzero argument
	f(i) = sin(N*t(i)) ./ (N * f(i));
	f(~i) = 1;

% sinc version
else
%	f = sinc(k / (K/N));
	f = nufft_sinc(k / (K/N));
end

% this is faster than matlab's sinc because it does not use "find"
function y = nufft_sinc(x)
y = ones(size(x));
%i = abs(x) > 1e-12;
i = x ~= 0;
x = x(i);
y(i) = sin(pi*x) ./ (pi*x);


function a = r8gd_indicator ( n, ndiag, offset )

%*****************************************************************************80
%
%% R8GD_INDICATOR sets up a R8GD indicator matrix.
%
%  Discussion:
%
%    The R8GD storage format is suitable for matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0.
%    Each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%    Similarly, the subdiagonals are assigned offsets of -1 through -(N-1).
%
%    Now, assuming that only a few of these diagonals contain nonzeros,
%    then for the I-th diagonal to be saved, we stored its offset in
%    OFFSET(I), and its entries in column I of the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals of the matrix
%    that are stored in the array.
%    NDIAG must be at least 1, and no more than 2 * N - 1.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Output, real A(N,NDIAG), the R8GD matrix.
%
  fac = 10^( i4_log_10 ( n ) + 1 );

  for i = 1 : n
    for diag = 1 : ndiag
      j = i + offset(diag);
      if ( 1 <= j & j <= n )
        a(i,diag) = fac * i + j;
      else
        a(i,diag) = 0.0;
      end
    end
  end

  return
end


function coeffs = alias(coeffs, m)
%ALIAS   Alias Fourier coefficients on equally spaced grid.
%   ALIAS(C, M) aliases the Fourier coefficients stored in the column vector C
%   to have length M. If M > LENGTH(C), the coefficients are padded with zeros.
%   If C is a matrix of coefficients, each of the columns is aliased to length
%   M.
%
% See also PROLONG.

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

% Get the number of coefficients.
n = size(coeffs, 1);

% Pad with zeros:
if ( m > n )

    k = ceil((m-n)/2);
    z = zeros(k, size(coeffs, 2));

    % Need to handle the odd vs. even case separately.
    if ( ~mod(n, 2) ) % n even.

        % First, account for the asymmetry in the coefficients when n is even.
        % This will account for the cos(N/2) coefficient, which is stored
        % in the coeffs(n,:) entry, using properties of the complex
        % exponential.
        coeffs = [ coeffs(1,:)/2; coeffs(2:n,:); coeffs(1,:)/2 ];
        coeffs = [ z; coeffs; z(1:end-1,:) ];

        % Next, check if m is odd. If it is, then coeffs is too long and we
        % need to remove the first row (the lowest degree
        % coefficients).
        if ( mod(m, 2) ) % m odd.
            coeffs = coeffs(2:end,:);
        end

    else % n odd.

        % There is no asymmetry in the coefficients, just pad them.
        coeffs = [ z; coeffs; z ];

        % Only need to check if m is even, in which case coeffs is too
        % long and we need to remove the last row (the highest degree
        % coefficients).
        if ( ~mod(m, 2) ) % m even.
            coeffs = coeffs(1:end-1,:);
        end
    end

    return

end

% If the number of coefficients is even then we extend them by one so they
% are odd by exploiting the symmetry property.  This makes the code below a
% little cleaner since fewer cases need to be handled.
if ( ~mod(n, 2) ) % n even.
    coeffs(1,:) = 0.5*coeffs(1,:);
    coeffs = [ coeffs; coeffs(1,:) ];
    n = n + 1;
end

% Need to handle the odd and even case (for m) differently.
if ( mod(m, 2) ) % m is odd.

    if ( m == 1 )

        % Reduce to a single point:
        constCoeffs = coeffs((n-1)/2+1,:);
        posCoeffs = coeffs((n-1)/2:-1:1,:);
        negCoeffs = coeffs((n-1)/2+2:n,:);
        e = ones(1, ceil((n-1)/2));
        e(1:2:end) = -1;
        coeffs = constCoeffs + (e*posCoeffs + e*negCoeffs);

    else

        m2 = (m-1)/2;
        n2 = (n-1)/2;
        % Extract coefficients:
        aliasedCoeffs = coeffs(n2-m2+1:n2+m2+1,:);

        % The code below aliases the coefficients from the higher modes
        % onto the lower modes. The principle behind the formula is figure
        % out which of the higher Fourier modes are indistinguishable from
        % the lower Fouirer modes on the grid consisting of m equally
        % spaced points from [-1,1). In general, when m and n are odd the
        % following will be equal for j=-(n-1)/2 to -(m+1)/2
        % exp(1i*pi*j*x) = sgn*exp(1i*pi*k*x) where
        % k = mod(j+(m+1)/2, -m) + (m-1)/2 and sgn = (-1)^mod(j+k, 2);
        for j = -n2:-m2-1
            k = mod(j+m2+1, -m) + m2;
            coeffIndexK = k + m2 + 1;  % Index into aliasedCoeffs for mode k.
            coeffIndexJ = j + n2 + 1;  % Index into coeffs for mode j.
            sgn = (-1)^mod(j+k, 2);
            aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + sgn*coeffs(coeffIndexJ,:);
            coeffIndexK = -k + m2 + 1;
            coeffIndexJ = -j + n2 + 1;
            aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + sgn*coeffs(coeffIndexJ,:);
        end
        coeffs = aliasedCoeffs;

    end

else % m is even.

    m2 = m/2;
    n2 = (n-1)/2;

    % Put the coefficient for the cos(m/2*pi*x) in the first entry of the
    % coefficient vector.  This corresopnds to the exp(-1i*pi*m/2*x).
    aliasedCoeffs = coeffs(n2+1-m2:n2+m2,:);
    % Extend the aliased cofficient vector so it is symmetric.  This allows
    % us to easily account for aliasing on the exp(-1i*pi*m/2*x) and
    % exp(1i*pi*m/2*x) term in the code below without the need for a
    % special if check.  The negative allows contribution for sin(m/2*pi*x)
    % to be removed.
    aliasedCoeffs = [ aliasedCoeffs; -aliasedCoeffs(1,:) ];

    % Follow a similar structure to the odd m case above.  The main
    % difference is that the higher order modes do not change sign when
    % aliased onto an even point grid.
    for j = -n2:-m2
        k = mod(j+m2,-m) + m2;
        coeffIndexK = k + m2 + 1;  % Index into aliasedCoeffs for mode k.
        coeffIndexJ = j + n2 + 1;  % Index into coeffs for mode j.
        aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + coeffs(coeffIndexJ,:);
        coeffIndexK = -k + m2 + 1;
        coeffIndexJ = -j + n2 + 1;
        aliasedCoeffs(coeffIndexK,:) = aliasedCoeffs(coeffIndexK,:) + coeffs(coeffIndexJ,:);
    end
    % Collapse the aliased coefficient vector down to an even number of
    % terms by adding in the aliasing of the exp(1i*pi*m/2*x) terms to the
    % exp(-1i*pi*m/2*x).
    aliasedCoeffs(1,:) = aliasedCoeffs(1,:) + aliasedCoeffs(end,:);
    aliasedCoeffs(end,:) = [];

    coeffs = aliasedCoeffs;

end

end


function [r,s,t,w] = threed_gauss(rule)
%-----------------------------------------------------------------------
%  threed_gauss.m - calculate Gauss integration points for tetrahedral
%                   elements
%
%  Copyright (c) 2002, Jeff Borggaard, Virginia Tech
%  Version: 1.0
%
%  Usage:    [r,s,t,w] = threed_gauss(rule)
%
%  Variables:     rule
%                        Number of Gauss points:
%                 r
%                        xi coordinate of Gauss points
%                 s
%                        eta coordinate of Gauss points
%                 t
%                        zeta coordinate of Gauss points
%                 w
%                        Gauss weights corresponding to (r,s,t)
%-----------------------------------------------------------------------

  if (rule == 1)
    % The following points correspond to a 1 point rule
    % Polynomials of degree 1 are integrated exactly
    r(1) = 0.25000000;  s(1) = 0.25000000;  t(1) = 0.25000000;

    w(1) = 0.16666666;  % (the volume of a regular tet is 1/6)

  elseif (rule == 4)
    % The following points correspond to a 4 point rule
    % Polynomials of degree 2 are integrated exactly
    r    = zeros(4,1);  s    = zeros(4,1);  t    = zeros(4,1);
    r(1) = 0.13819660;  s(1) = 0.13819660;  t(1) = 0.13819660;
    r(2) = 0.58541020;  s(2) = 0.13819660;  t(2) = 0.13819660;
    r(3) = 0.13819660;  s(3) = 0.58541020;  t(3) = 0.13819660;
    r(4) = 0.13819660;  s(4) = 0.13819660;  t(4) = 0.58541020;

    w    = 0.041666666667*ones(4,1);

  elseif (rule == 5)
    % The following points correspond to a 5 point rule
    % Polynomials of degree 3 are integrated exactly
    r    = zeros(5,1);  s    = zeros(5,1);  t    = zeros(5,1);
    r(1) = 0.25000000000;  s(1) = 0.25000000000;  t(1) = 0.25000000000;
    r(2) = 0.16666666666;  s(2) = 0.16666666666;  t(2) = 0.16666666666;
    r(3) = 0.50000000000;  s(3) = 0.16666666666;  t(3) = 0.16666666666;
    r(4) = 0.16666666666;  s(4) = 0.50000000000;  t(4) = 0.16666666666;
    r(5) = 0.16666666666;  s(5) = 0.16666666666;  t(5) = 0.50000000000;

    w    = zeros(5,1);
    w(1) =-0.13333333333;
    w(2) = 0.07500000000;
    w(3) = 0.07500000000;
    w(4) = 0.07500000000;
    w(5) = 0.07500000000;

  else
    error('quadrature rules other than 1, 4 or 5 are not supported\n');
    keyboard
  end


function value = r8_gamr ( x )

%*****************************************************************************80
%
%% R8_GAMR evaluates the reciprocal gamma function of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the value of the reciprocal gamma
%    function at X.
%
  if ( x <= 0.0 && r8_aint ( x ) == x )

    value = 0.0;

  elseif ( abs ( x ) <= 10.0 )

    value = 1.0 / r8_gamma ( x );

  else

    [ alngx, sgngx ] = r8_lgams ( x );
    value = sgngx * exp ( - alngx );

  end

  return
end


function a = dif2_llt ( n )

%*****************************************************************************80
%
%% DIF2_LLT returns the Cholesky factor of the DIF2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    a(i,i) = sqrt ( i + 1 ) / sqrt ( i );
  end

  for i = 2 : n
    a(i,i-1) = - sqrt ( i - 1 ) / sqrt ( i );
  end

  return
end


function [ x, seed ] = gompertz_sample ( a, b, seed )

%*****************************************************************************80
%
%% GOMPERTZ_SAMPLE samples the Gompertz PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    1 < A, 0 < B.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ cdf, seed ] = r8_uniform_01 ( seed );

  x = gompertz_cdf_inv ( cdf, a, b );

  return
end


function [TKenergy]=TKEO(x)
%
%% Utility function to calculate measures based on the nonlinear energy operator
%
% Function to estimate the TKEO of input data, follows the classical rule
% of Teager and Kaiser
%
% Inputs:  x            -> any time-series signal (vector)
%
% =========================================================================
% Output:  TKenergy     -> (nonlinear) Teager-Kaiser Energy Operator (TKEO)
% =========================================================================
%
% Part of the "Speech Disorders" Toolbox
%
% -----------------------------------------------------------------------
% Useful references:
%
% 1) J. Kaiser: On a simple algorithm to calculate the 'energy' of a
%    signal, Proc. IEEE International Conference on Acoustics, Speech, and
%    Signal Processing (ICASSP '90), pp. 381-384, Albuquerque, NM, USA,
%    April 1990
%
% -----------------------------------------------------------------------
%
% Last modified on 24 August 2014
%
% Copyright (c) Athanasios Tsanas, 2014
%
% ********************************************************************
% If you use this program please cite:
%
% 1) A. Tsanas: "Accurate telemonitoring of Parkinson's disease symptom
%    severity using nonlinear speech signal processing and statistical
%    machine learning", D.Phil. thesis, University of Oxford, 2012
% ********************************************************************
%
% For any question, to report bugs, or just to say this was useful, email
% tsanasthanasis@gmail.com

%% Algorithm computation

% The algorithm is computed using the instantaneous value squared minus the
% previous step value times the next step value:[x_n]^2-[x_(n-1)]*(x_(n+1)]
%
%
% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details.

% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details.
%

data_length=length(x);
TKenergy=zeros(data_length,1);

TKenergy(1)=(x(1))^2; % first sample

for n=2:data_length-1
    TKenergy(n)=(x(n))^2-x(n-1)*x(n+1);
end

%  TKenergy(2:data_length-1) = x(2:data_length-1).^2 - x(1:data_length-2).*x(3:data_length); % alternative, vectorized version

TKenergy(data_length)=(x(data_length))^2; % last sample


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
%   Unit step sequence

%  u[n]
 n1=-3:-1;
 n2=0:5;
 n=[n1 n2];
 u1=zeros(size(n1));
 u2=ones(size(n2));
 u=[u1 u2];
 stem(n,u)

%  second way
 figure
 n=-3:5
 n0=0;
 u=(n>=n0)
 stem(n,u)

 %u[n-n0]
 figure
 n1=-3:1;
 n2=2:5;
 n=[n1 n2];
 u1=zeros(size(n1));
 u2=ones(size(n2));
 u2=ones(size(n2));
 u=[u1 u2];
 stem(n,u)

%  second way
 figure
 n=-3:5
 n0=2;
 u=((n-n0)>=0)
 stem(n,u)


function S = randskew(n, N)
% Generates random skew symmetric matrices with normal entries.
%
% function S = randskew(n)
% function S = randskew(n, N)
%
% S is an n-by-n-by-N array where each slice S(:, :, i) for i = 1..N is a
% random skew-symmetric matrix with upper triangular entries distributed
% independently following a normal distribution (Gaussian, zero mean, unit
% variance).
%
% By default, N = 1.
%
% See also: randrot randsym randskewh

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sept. 25, 2012.
% Contributors:
% Change log:
%       June 19, 2019 (NB):
%           Now handles the case n = 1 properly.

    if nargin < 2
        N = 1;
    end

    if n == 1
        S = zeros(1, 1, N);
        return;
    end

    % Subindices of the (strictly) upper triangular entries of an n-by-n
    % matrix
    [I, J] = find(triu(ones(n), 1));

    K = repmat(1:N, n*(n-1)/2, 1);

    % Indices of the strictly upper triangular entries of all N slices of
    % an n-by-n-by-N matrix
    L = sub2ind([n n N], repmat(I, N, 1), repmat(J, N, 1), K(:));

    % Allocate memory for N random skew matrices of size n-by-n and
    % populate each upper triangular entry with a random number following a
    % normal distribution and copy them with opposite sign on the
    % corresponding lower triangular side.
    S = zeros(n, n, N);
    S(L) = randn(size(L));
    S = S - multitransp(S);

end


function value = box_01_contains_point_nd ( dim_num, p )

%*****************************************************************************80
%
%% BOX_01_CONTAINS_POINT_ND determines if a point is inside a unit box in ND.
%
%  Discussion:
%
%    A unit box is assumed to be a rectangle with sides aligned on coordinate
%    axes.  It can be described by its low and high corner, P1 and P2:
%
%      0 <= P(1:DIM_NUM) <= 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real P(DIM_NUM), the point to be checked.
%
%    Output, logical VALUE, is TRUE if P is inside the box.
%
  value = 0;

  for i = 1 : dim_num
    if ( p(i) < 0.0 | 1.0 < p(i) )
      return
    end
  end

  value = 1;

  return
end


function [mu,P,w,cost]=kMeanspp(z,K,maxIter)
%%KMEANSPP Run the k-means++ algorithm for clustering a set of more than K
%          data points into k clusters. The k-means++ algorithm is the
%          same as the general k-means algorithm, except a different
%          initialization routine is used.
%
%INPUTS: z A zDim X numPoints set of vectors that are to be clustered.
%        K The number of clusters to form. K>=numPoints.
%  maxIter The maximum number of iterations to perform for clustering. If
%          omitted, maxIter=1000;
%
%OUTPUTS: mu A zDim X K set of the cluster means.
%          P A zDim X zDim X K set of sample covariance matrices for each
%            of the k clusters, where mu(:,n) is the mean of the nth
%            cluster.
%          w A KX1 vector of weights such that w(n) is the fraction of the
%            total original points assigned to the nth cluster.
%       cost The cost of the k-means assignment. This is the sum of the
%            squared distances between the points assigned to a cluster and
%            the cluster mean.
%
%The k-means algorithm is a suboptimal algorithm that tries to find a set
%of k-means such that the sum of the squared distances from the points to
%the closest means is minimized. The implementation here uses a better
%initialization the k-means++ initialization from [1], which improves the
%probability that the algorithm will converge to a good solution.
%
%Note that the k-means algorithm used a randomized initialization, so one
%will not always get the same results when the function is run twice on the
%same data.
%
%REFERENCES:
%[1] D. Arthur and S. Vassilvitskii, "k-means++: The advantages of careful
%    seeding," in Proceedings of the Eighteenth Annual ACM-SIAM Symposium
%    on Discrete Algorithms, New Orleans, LA, Jan. 2007, pp. 1027-1035.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3)
   maxIter=1000;
end

%This uses the k-mean++ initalization algorithm on the given data.
%K is the number of clusters to form.

zDim=size(z,1);
numPoints=size(z,2);

mu=zeros(zDim,K);

%Use k-mean++ initialization.
%The first center is chosen randomly.
mu(:,1)=z(:,randi([1;numPoints]));
diff=mu(:,1)*ones(1,numPoints)-z;
nearestMuDist=sum(diff.*diff,1);%The squared distances from this point.

for k=2:K
    %The points are chosen with a probability equal to the ratio of the
    %squared distance to the sum of all distances.
    PMF=nearestMuDist/sum(nearestMuDist);%The PMF from which we will draw.
    CMF=cumsum(PMF);

    %We have to find the first element in the CMF that is greater than a
    %random draw; that will be the next center.
    idx=sum(CMF<rand(1))+1;
    %idx=randi([1;numPoints]);%For the regular k-means.
    %Set the center and update the nesrest center distances.
    mu(:,k)=z(:,idx);
    diff=mu(:,k)*ones(1,numPoints)-z;
    nearestMuDist=min(nearestMuDist,sum(diff.*diff,1));
end

%The k-means refinement.
muPrev=ones(zDim,K)*Inf;
numIter=0;
while(sum(sum(muPrev~=mu))~=0&&numIter<maxIter)
    %Calculate the distances between the points and the current set of
    %means.
    minCosts=ones(1,numPoints)*Inf;
    selClust=zeros(1,numPoints);
    for k=1:K
        diff=bsxfun(@minus,z,mu(:,k));%mu(:,k)*ones(1,numPoints)-z;
        dist=sum(diff.*diff,1);%The squared distances from this center.

        sel=dist<minCosts;
        minCosts(sel)=dist(sel);
        %Assign those points to this cluster if it has the lowest cost.
        selClust(sel)=k;
    end

    %Now, all of the points are assigned to clusters, and we can calculate
    %a new set of means.
    muPrev=mu;
    for k=1:K
        mu(:,k)=mean(z(:,selClust==k),2);
    end
    numIter=numIter+1;
end

%The clustering is done. We will now also return the weights and covariance
%estimates. The weights will be the fraction of the total points assigned
%to a given cluster. The covariance will be the sample covariance.
if(nargout>1)
    w=zeros(K,1);
    P=zeros(zDim,zDim,K);
    for k=1:K
        sel=selClust==k;
        w(k)=sum(sel);

        zSel=z(:,sel);

        diff=bsxfun(@minus,zSel,mu(:,k));
        P(:,:,k)=diff*diff'/w(k);
    end
    w=w/sum(w);

    cost=sum(minCosts)/numPoints;
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [R_ned_2_inert, ned_x, ned_y, ned_z] = computeNedFrame(ut, rVectECI, bodyInfo)
    %Source: https://en.wikipedia.org/wiki/North_east_down

    [phi, lambda, ~, ~, ~, ~, ~, ~, REci2Ecef] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo);

    REcef2Ned = [-sin(phi)*cos(lambda), -sin(lambda), -cos(phi)*cos(lambda);
                 -sin(phi)*sin(lambda),  cos(lambda), -cos(phi)*sin(lambda);
                  cos(phi),              0,           -sin(phi)]';

    R_ned_2_inert = (REcef2Ned * REci2Ecef)';
    ned_x = R_ned_2_inert(:,1);
    ned_y = R_ned_2_inert(:,2);
    ned_z = R_ned_2_inert(:,3);
end


function a = stripe ( n )

%*****************************************************************************80
%
%% STRIPE returns the STRIPE matrix.
%
%  Example:
%
%    N = 7
%
%    5  2  1  1  .  .  .
%    2  6  3  1  1  .  .
%    1  3  6  3  1  1  .
%    1  1  3  6  3  1  1
%    .  1  1  3  6  3  1
%    .  .  1  1  3  6  2
%    .  .  .  1  1  2  5
%
%  Properties:
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A is symmetric: A' = A.
%
%    A is banded, with bandwidth 7.
%
%    A is centrosymmetric: A(I,J) = A(N+1-I,N+1-J).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

           if ( j == i - 3 )
        a(i,j) = 1.0;
      elseif ( j == i - 2 )
        a(i,j) = 1.0;
      elseif ( j == i - 1 )
        if ( j == 1 | j == n - 1 )
          a(i,j) = 2.0;
        else
          a(i,j) = 3.0;
        end
      elseif ( j == i )
        if ( i == 1 | i == n )
          a(i,j) = 5.0;
        else
          a(i,j) = 6.0;
        end
      elseif ( j == i + 1 )
        if ( j == 2 | j == n )
          a(i,j) = 2.0;
        else
          a(i,j) = 3.0;
        end
      elseif ( j == i + 2 )
        a(i,j) = 1.0;
      elseif ( j == i + 3 )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


clear;  clc;
% P2.5
n = 0:10;
x = 10*exp(-j*0.4*pi*n);
[xe, xo, m] = evenoddcomplex(x,n);
subplot(3,2,1); stem(n,real(x),'ko'); title('Real Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(3,2,3); stem(m,real(xe),'ko'); title('Real Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(3,2,5); stem(m,real(xo),'ko'); title('Real Odd Part')
xlabel('n'); ylabel('xo(n)');
subplot(3,2,2); stem(n,imag(x),'ko'); title('Imaginary Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(3,2,4); stem(m,imag(xe),'ko'); title('Imaginary Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(3,2,6); stem(m,imag(xo),'ko'); title('Imaginary Odd Part')
xlabel('n'); ylabel('xo(n)');


function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha)

%project_points2.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points_fisheye(X,om,T,f,c,k,alpha)
%
%Projects a 3D structure onto the image plane of a fisheye camera.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion fisheye coefficients (5x1 vector)
%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x5 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2,
%call theta = atan(r),
%Fisheye distortion -> theta_d = theta * (1 + k(1)*theta^2 + k(2)*theta^4 + k(3)*theta^6 + k(4)*theta^8)
%
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = (theta_d / r) * x
%yy = (theta_d / r) * y
%
%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*(xx + alpha*yy) + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

if nargin < 7,
    alpha = 0;
    if nargin < 6,
        k = zeros(4,1);
        if nargin < 5,
            c = zeros(2,1);
            if nargin < 4,
                f = ones(2,1);
                if nargin < 3,
                    T = zeros(3,1);
                    if nargin < 2,
                        om = zeros(3,1);
                        if nargin < 1,
                            error('Need at least a 3D structure to project (in project_points.m)');
                            return;
                        end;
                    end;
                end;
            end;
        end;
    end;
end;

[m,n] = size(X);

if nargout > 1,
    [Y,dYdom,dYdT] = rigid_motion(X,om,T);
else
    Y = rigid_motion(X,om,T);
end;

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

if nargout > 1,
    dxdom = zeros(2*n,3);
    dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
    dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

    dxdT = zeros(2*n,3);
    dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
    dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);
end;

% Add fisheye distortion:

r2 = x(1,:).^2 + x(2,:).^2;

if nargout > 1,
    dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
    dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);
end;

% Radial distance:
r = sqrt(r2);
if nargout > 1,
    drdr2 = ones(1,length(r));
    drdr2(r>1e-8) = 1 ./ (2*r(r>1e-8));

    drdom = [ (drdr2').*dr2dom(:,1)   (drdr2').*dr2dom(:,2)  (drdr2').*dr2dom(:,3)  ];
    drdT = [ (drdr2').*dr2dT(:,1)   (drdr2').*dr2dT(:,2)  (drdr2').*dr2dT(:,3)  ];
end;

% Angle of the incoming ray:
theta = atan(r);
if nargout > 1,
    dthetadr = 1 ./ (1 + r2);

    dthetadom = [ (dthetadr').*drdom(:,1)   (dthetadr').*drdom(:,2)  (dthetadr').*drdom(:,3) ];
    dthetadT = [ (dthetadr').*drdT(:,1)   (dthetadr').*drdT(:,2)  (dthetadr').*drdT(:,3) ];
end;

% Add the fisheye distortion:

theta2 = theta.^2;
theta3 = theta2.*theta;
theta4 = theta2.^2;
theta5 = theta4.*theta;
theta6 = theta3.^2;
theta7 = theta6.*theta;
theta8 = theta4.*theta4;
theta9 = theta8.*theta;

% Fisheye distortion -> theta_d = theta * (1 + k(1)*theta2 + k(2)*theta4 + k(3)*theta6 + k(4)*theta8)

theta_d = theta + k(1)*theta3 + k(2)*theta5 + k(3)*theta7 + k(4)*theta9;

if nargout > 1,
    dtheta_ddtheta = 1 + 3*k(1)*theta2 + 5*k(2)*theta4 + 7*k(3)*theta6 + 9*k(4)*theta8;
    dtheta_ddom = [ (dtheta_ddtheta').*dthetadom(:,1)   (dtheta_ddtheta').*dthetadom(:,2)  (dtheta_ddtheta').*dthetadom(:,3) ];
    dtheta_ddT = [ (dtheta_ddtheta').*dthetadT(:,1)   (dtheta_ddtheta').*dthetadT(:,2)  (dtheta_ddtheta').*dthetadT(:,3) ];
    dtheta_ddk = [theta3' theta5' theta7' theta9'];
end;

% ratio:
inv_r = ones(1,length(r));
inv_r(r>1e-8) = 1./r(r>1e-8);

cdist = ones(1,length(r));
cdist(r > 1e-8) = theta_d(r > 1e-8) ./ r(r > 1e-8);

if nargout > 1,
    dcdistdom = [ ((inv_r').*(dtheta_ddom(:,1) - (cdist').*drdom(:,1)))  ((inv_r').*(dtheta_ddom(:,2) - (cdist').*drdom(:,2)))   ((inv_r').*(dtheta_ddom(:,3) - (cdist').*drdom(:,3))) ];
    dcdistdT = [ ((inv_r').*(dtheta_ddT(:,1) - (cdist').*drdT(:,1)))  ((inv_r').*(dtheta_ddT(:,2) - (cdist').*drdT(:,2)))   ((inv_r').*(dtheta_ddT(:,3) - (cdist').*drdT(:,3))) ];
    dcdistdk = [ (inv_r'.*dtheta_ddk(:,1)) (inv_r'.*dtheta_ddk(:,2)) (inv_r'.*dtheta_ddk(:,3))  (inv_r'.*dtheta_ddk(:,4)) ];
end;

xd1 = x .* (ones(2,1)*cdist);

if nargout > 1,
    dxd1dom = zeros(2*n,3);
    dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
    dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
    coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
    dxd1dom = dxd1dom + coeff.* dxdom;

    dxd1dT = zeros(2*n,3);
    dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
    dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
    dxd1dT = dxd1dT + coeff.* dxdT;

    dxd1dk = zeros(2*n,4);
    dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk;
    dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk;
end;

% No tangential distortion:
xd2 = xd1;
if nargout > 1,
    dxd2dom = dxd1dom;
    dxd2dT = dxd1dT;
    dxd2dk = dxd1dk;
end;

% Add Skew:
xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];

% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha
if nargout > 1,
    dxd3dom = zeros(2*n,3);
    dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
    dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
    dxd3dT = zeros(2*n,3);
    dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
    dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
    dxd3dk = zeros(2*n,4);
    dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
    dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
    dxd3dalpha = zeros(2*n,1);
    dxd3dalpha(1:2:2*n,:) = xd2(2,:)';
end;

% Pixel coordinates:
if length(f)>1,
    xp = xd3 .* (f * ones(1,n))  +  c*ones(1,n);
    if nargout > 1,
        coeff = reshape(f*ones(1,n),2*n,1);
        dxpdom = (coeff*ones(1,3)) .* dxd3dom;
        dxpdT = (coeff*ones(1,3)) .* dxd3dT;
        dxpdk = (coeff*ones(1,4)) .* dxd3dk;
        dxpdalpha = (coeff) .* dxd3dalpha;
        dxpdf = zeros(2*n,2);
        dxpdf(1:2:end,1) = xd3(1,:)';
        dxpdf(2:2:end,2) = xd3(2,:)';
    end;
else
    xp = f * xd3 + c*ones(1,n);
    if nargout > 1,
        dxpdom = f  * dxd3dom;
        dxpdT = f * dxd3dT;
        dxpdk = f  * dxd3dk;
        dxpdalpha = f .* dxd3dalpha;
        dxpdf = xd3(:);
    end;
end;

if nargout > 1,
    dxpdc = zeros(2*n,2);
    dxpdc(1:2:end,1) = ones(n,1);
    dxpdc(2:2:end,2) = ones(n,1);
end;

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(4,1);
alpha = 0.01*randn(1,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha);

% Test on om: not OK

dom = 0.00000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points_fisheye(X,om2,T,f,c,k,alpha);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: not OK

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points_fisheye(X,om,T2,f,c,k,alpha);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points_fisheye(X,om,T,f2,c,k,alpha);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points_fisheye(X,om,T,f,c2,k,alpha);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.00001 * norm(k)*randn(4,1);
k2 = k + dk;

[x2] = project_points_fisheye(X,om,T,f,c,k2,alpha);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on alpha: OK!!

dalpha = 0.001 * norm(k)*randn(1,1);
alpha2 = alpha + dalpha;

[x2] = project_points_fisheye(X,om,T,f,c,k,alpha2);

x_pred = x + reshape(dxdalpha * dalpha,2,n);

norm(x2-x)/norm(x2 - x_pred)


function [phi, theta, psi] = rotMatXYZ2Eul(rotMat)
    % Function to convert rotation matrix to Euler angles (extrinsic x-y-z convention).
    % Used in yaw to convert between global and local reference frames.
    %
    % Parameters
    % ------------
    %     rotMat : float array [3,3]
    %         Rotational matrix to transform vectors to the new frame
    %
    % Returns
    % ------------
    %     phi : float [rad]
    %         Angle of rotation about the x-axis
    %     theta : float [rad]
    %         Angle of rotation about the y-axis
    %     psi : float [rad]
    %         Angle of rotation about the z-axis

    phi = atan2(-rotMat(2,3), rotMat(3,3));
    theta = asin(rotMat(1,3));
    psi = atan2(-rotMat(1,2), rotMat(1,1));
end


%% Equation: Poisson Equation Discretized by $BDM_1$ Element in 2D
% We explain the assembling of the matrix equation for the lowest order BDM element
% discretization of Poisson equation.
%
% [u,sigma] = PoissonBDM1(node,elem,bdEdge,f,g_D,varargin)
%
% Created by Ming Wang at Jan 2. 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Data Structure
%   [elem2dof,dofSign,edge] = dofRT0(elem);
%
% will construct local to global index map; see ifem dofBDM1doc for
% details.
%
%% Local Bases
% Suppose [i,j] is the k-th edge. The hierarchic basis along with
% their div are given by
%
% $$ \phi_k = \lambda_i rot \lambda_j - \lambda_j rot \lambda_i,\quad
%    \psi_k = \lambda_i rot \lambda_j + \lambda_j rot \lambda_i. $$
%
% $$ div \phi_k = 2 \nabla \lambda_i \cdot rot \lambda_j, \quad div \psi_k = 0.$$
%
% Inside one triangular, the 6 bases functions along with their div
% corresponding to 3 local edges [2 3; 3 1; 1 2] are:
%
% $$ \phi_1 = \lambda_2 rot \lambda_3 - \lambda_3 rot \lambda_2,\quad
%    \psi_1 = \lambda_2 rot \lambda_3 + \lambda_3 rot \lambda_2,\quad
%    div\phi_1 = 2 \nabla \lambda_2 \cdot rot \lambda_3, \quad div \psi_1 = 0. $$
%
% $$ \phi_2 = \lambda_3 rot \lambda_1 - \lambda_1 rot \lambda_3,\quad
%    \psi_2 = \lambda_3 rot \lambda_1 + \lambda_1 rot \lambda_3,\quad
%    div\phi_2 = 2 \nabla \lambda_3 \cdot rot \lambda_1, \quad div \psi_2 = 0. $$
%
% $$ \phi_3 = \lambda_1 rot \lambda_2 - \lambda_2 rot \lambda_1,\quad
%    \psi_3 = \lambda_1 rot \lambda_2 + \lambda_2 rot \lambda_1,\quad
%    div\phi_3 = 2 \nabla \lambda_1 \cdot rot \lambda_2, \quad div \psi_3 = 0. $$
%
% Locally, we order the local bases in the following way:
%
% $$\{\phi_1,~\,\phi_2,~\,\phi_3,~\,\psi_1,~\,\psi_2,~\, \psi_3.\}$$
%
% Note that $RT_0 \subset BDM_1$, and $\{ \phi_1,~\,\phi_2,~\,\phi_3 \}$ is the
% local bases for $RT_0$.
%
% Because of the different oritentation of local and global faces, from
% local bases to the global one, the direction should be corrected. That is
%
%    phiGlobal(elem2dof(t,1),:) = phi(t,1)*dofSign(t,1);

%% Mass Matrix
% We use the integral formula
%
% $$ \int_T
% \lambda_1^{\alpha_1}\lambda_2^{\alpha_2}\lambda_3^{\alpha_3}
% dx = \frac{\alpha_1!\alpha_2!\alpha_3!2!}{(\sum _{i=1}^3\alpha_i
% + 2)!}\;|T|,$$
%
% to get
%
% $$ \int _T\lambda_i\lambda_j dx = (1+(i==j))|T|/12. $$
%
% For two local bases $\phi_i$ and $\phi_j$ corresponding to the ith and
% jth local edges, suppose i = [i1 i2], j = [j1 j2].
% $$\int_T \phi_i \phi_j dx = \int_T (
%  \lambda_{i1} rot \lambda_{i2}\cdot\lambda_{j1} rot \lambda_{j2}
% -\lambda_{i2} rot \lambda_{i1}\cdot\lambda_{j1} rot \lambda_{j2}
% -\lambda_{i1} rot \lambda_{i2}\cdot\lambda_{j2} rot \lambda_{j1}
% +\lambda_{i2} rot \lambda_{i1}\cdot\lambda_{j2} rot \lambda_{j1})dx
% $$
%
% $\int_T \psi_i \psi_j dx$ and $\int_T \psi_i \phi_j dx$ can be computed similarly.
%
%% Matrix for Differential Operator
% Note that $\nabla \cdot \psi = 0$, We only need to record $\nabla \cdot \phi_i$ and
% then the computation $\int _T \nabla
% \cdot \phi_i dx$ is straightforward. Just remember to correct the direction.

%% Right hand side
% We use 5-points quadrature which is exact for cubic polynomials. In the
% barycentric coordinate, the 5-points are
%
% $$ p_1 = [2/3, 1/6, 1/6], \quad  w_1 = 1/3 $$
%
% $$ p_2 = [1/6, 2/3, 1/6], \quad  w_2 = 1/3 $$
%
% $$ p_3 = [1/6, 1/6, 2/3], \quad  w_3 = 1/3 $$
%
%%
% Note that the two for loops are nested in such a way that the point pxy
% and the evulation Jp is just computed once.
%
% The local to global assembling is computed using accumarray
%
%   b = accumarray(elem2dof(:),bt(:),[Ndof 1]);


function [x C d] = project_affine(v, A, b, C, d)
% PROJECT_AFFINE    Project a point into an affine set.
%
%   project_affine(v,A,b) is the projection of v onto
%   the affine set { x | Ax = b }.
%
%   You can also call the function as
%
%     [x C d] = project_affine(v,A,b);
%
%   and then call it again with different argument v2 via
%
%     x2 = project_affine(v2,A,b,C,d);
%
%   If calling the function repeatedly with the same A and b,
%   all evaluations after the initial one will be much faster
%   when passing in the cached values C and d.

    if exist('C','var') && ~isempty(C) && exist('d','var') && ~isempty(d)
        x = C*v + d;
    else
        % x = v - pinv(A)*(A*v - b);
        pA = pinv(A);
        C = eye(length(v)) - pA*A;
        d = pA*b;
        x = C*v + d;
    end
end


function x_opt = coordinate_search ( x, function_handle, flag )

%*****************************************************************************80
%
%% COORDINATE_SEARCH carries out a direct search minimization algorithm.
%
%  Discussion:
%
%    This function implements a direct search algorithm, to seek a
%    multidimensional point X_OPT which minimizes a scalar function F(X).
%
%    The function is currently set up for coordinate search by
%    using a default stencil along coordinate axes.  The stencil and the
%    initial size of the stencil (delta) should probably be input parameters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2009
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, real X(N_VAR), a starting estimate for the minimizer.
%
%    Input, function handle FUNCTION_HANDLE ( X ), ...
%
%    Optional input, integer FLAG, selects features of the algorithm.
%    If FLAG is not input, it defaults to the value 0.
%    0, do not include graphical displays.
%    nonzero, display the status of the search graphically.
%    3, use the water stick stencil.
%
%    Output, real X_OPT(N_VAR), the estimated minimizer.
%
%  Local Parameters:
%
%    Local, real DELTA, the initial size of the stencil.
%
%    Local, real TOLERANCE, a tolerance for the variation between the maximum
%    and minimum values of F in the stencil.
%
%    Local, integer MAX_FEVAL, the maximum number of function evaluations.
%
  delta = 1.0;
  tolerance = 1.0E-06;
  max_feval = 250;
  verbose = 0;
%
%  The default value of FLAG is 0.
%
  if ( nargin <= 2 )
    flag = 0;
  end

  x = x(:)';
  n_var = length(x);
%
%  Define the search stencil
%
  if ( flag ~= 3 )

    for i = 1 : n_var
      stencil(2*i-1).v    = zeros(size(x));
      stencil(2*i-1).v(i) = 1;
      stencil(2*i  ).v    = zeros(size(x));
      stencil(2*i  ).v(i) =-1;
    end

  else

    stencil(1).v = [   1        0    ];
    stencil(2).v = [ -1/2  sqrt(3)/2 ];
    stencil(3).v = [ -1/2 -sqrt(3)/2 ];

  end

  n_sten = length ( stencil );
%
%  Compute the function value at the initial point.
%
  f = feval ( function_handle, x );
  n_feval = 1;

  if ( flag )
    xp = linspace ( -5.0, 5.0, 101 );
    yp = xp;
    for i=1:101
      for j=1:101
        fp(j,i) = feval(function_handle,[xp(i),yp(j)]);
      end
    end

    figure(27)
    hold on
    contour(xp,yp,fp,linspace(0,200,25))
  end
%
%  Begin the Direct Search iteration.
%
  while ( 1 )

    x_s = compute_points ( x, stencil, delta );
%
%  Plot the current search pattern.
%
    if ( flag )
      for i=1:n_sten
        plot([x(1) x_s(i,1)],[x(2) x_s(i,2)],'r')
        plot(x_s(i,1),x_s(i,2),'r*')
      end
    end

    f_s = evaluate ( x_s, function_handle );
    n_feval = n_feval + n_sten;
%
% "erase" the current search pattern after it has been considered
%
    if ( flag )

      for i = 1 : n_sten
        plot ( [x(1) x_s(i,1)],[x(2) x_s(i,2)],'b')
        plot ( x_s(i,1),x_s(i,2),'b*')
      end
    end
%
%  Find the element in the pattern with the lowest function value.
%
    [ fc_min, i_min ] = min ( f_s );
    [ fc_max        ] = max ( f_s );
%
%  If we found a lower value, move to that location.
%
    if ( fc_min < f )

      xb = x_s(i_min,:);
      shift = xb - x;

      x = xb;
      f = fc_min;

      for i = 1 : n_sten
        x_s(i,:) = x_s(i,:) + shift;
      end
%
%  Otherwise, shrink the pattern about the center and recalculate the stencil.
%
    else
      delta = delta / 2.0;
      x_s = compute_points ( x, stencil, delta );
    end
%
%  Test whether we should continue.
%
    converged = ( fc_max - fc_min < tolerance ) || ( delta < tolerance );

    if ( converged )
      break
    end

    diverged = ( max_feval < n_feval );

    if ( diverged )
      break
    end

  end

  if ( verbose )
    fprintf ( 'The best point x^* was: %d %d\n', x(1,:) );
    fprintf ( 'f(x^*) = %d\n', f(1) );
  end

  x_opt = x(1,:);

  if ( verbose )
    fprintf ( 'The algorithm terminated after %d function evaluations\n',...
      n_feval);
  end

  if ( diverged )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Convergence was not achieved.\n' );
    fprintf ( 1, '  The iteration limit of %d was exceeded.\n', max_feval );
  end

  return
end
function f = evaluate ( x, function_handle )

%*****************************************************************************80
%
%% EVALUATE evaluates the function at the stencil points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2009
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, real X(N_STEN,N_VAR), a set of N_STEN points, each of
%    dimension N_VAR.
%
%    Input, f = FUNCTION_HANDLE ( x ), the handle of a function which
%    accepts as input a vector X and returns the scalar function value F.
%
%    Output, real F(N_STEN), the function evaluated at each of the points.
%
  [ n_sten, n_var ] = size ( x );

  f = zeros ( 1, n_sten );

  for i = 1 : n_sten
    f(i) = feval ( function_handle, x(i,:) );
  end

  return
end
function x_s = compute_points ( x, stencil, delta )

%*****************************************************************************80
%
%% COMPUTE_POINTS computes all of the points in the stencil.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2009
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, real X(N_VAR), the center of the stencil.
%
%    Input, real STENCIL, a structure describing the stencil.
%
%    Input, real DELTA, determines the geometric size of the stencil.
%
%    Output, real X_S(N_STEN,N_VAR), the stencil points.
%
  n_var = length(x);
  n_sten = length(stencil);

  x_s = zeros ( n_sten, n_var );

  for i = 1 : n_sten
    x_s(i,:) = x + delta * stencil(i).v;
  end

  return
end


function pts = cuboid_prm2pts(prm, vp)
% converts five parameters of cuboid to eight 2D points
%
% prm: [x, y, w, h, d]
% x,y: 2D coordinates of corner1 (top,left,front corner)
% w: length (in pixels) from corner1 to corner2
% h: length (in pixels) from corner1 to corner3
% d: length (in pixels) from corner1 to corner5
%
% pts: [8 x 2] 2D coordinates of eight points on cuboid
%   5---6   1------2
%  /   /|   |      |
% 1---2 8   3------4    ....
% |   |/     \    /
% 3---4       7--8
%
% 1: front,top,left
% 2: front,top,right
% ...
% 8: back,bottom,right

pts = zeros(8,2);

pts(1,:) = [prm(1) prm(2)];
pts(2,:) = towardsright(pts(1,:), vp, prm(3));
pts(3,:) = towardsdown(pts(1,:), vp, prm(4));
pts(5,:) = towardsback(pts(1,:), vp, prm(5));
pts(4,:) = line_intersect(pts(2,:), vp{1}, pts(3,:), vp{2});
pts(6,:) = line_intersect(pts(2,:), vp{3}, pts(5,:), vp{2});
pts(7,:) = line_intersect(pts(3,:), vp{3}, pts(5,:), vp{1});
pts(8,:) = mean(...
    [line_intersect(pts(4,:), vp{3}, pts(6,:), vp{1}); ...
     line_intersect(pts(7,:), vp{2}, pts(6,:), vp{1}); ...
     line_intersect(pts(4,:), vp{3}, pts(7,:), vp{2})], 1);