function fem2d_poisson_sparse ( prefix )

%*****************************************************************************80
%
%% MAIN is the main routine for FEM2D_POISSON_SPARSE.
%
%  Discussion:
%
%    This program uses MATLAB's sparse matrix storage, factorization
%    and solution facilities to compute an approximate solution to
%    the Poisson equation via the finite element method.
%
%    Three functions are changed from those in the FEM2D_POISSON code:
%    * the main program FEM2D_POISSON is replaced by FEM2D_POISSON_SPARSE.
%    * the routine ASSEMBLE_POISSON is replaced by ASSEMBLE_POISSON_SPARSE.
%    * the routine DIRICHLET_APPLY is replaced by DIRICHLET_APPLY_SPARSE.
%
%    This program solves the Poisson equation
%
%      -DEL H(X,Y) DEL U(X,Y) + K(X,Y) * U = F(X,Y)
%
%    in a triangulated region in the plane.
%
%    Along the boundary of the region, Dirichlet conditions
%    are imposed:
%
%      U(X,Y) = G(X,Y)
%
%    The code uses continuous piecewise linear basis functions on
%    triangles.
%
%  Problem specification:
%
%    The user defines the geometry by supplying two data files
%    which list the node coordinates, and list the nodes that make up
%    each element.
%
%    The user specifies the right hand side of the Dirichlet boundary
%    conditions by supplying a function
%
%      function node_bc = dirichlet_condition ( node_num, node_xy )
%
%    The user specifies the coefficient function H(X,Y):
%
%      function node_h = h_coef ( node_num, node_xy )
%
%    The user specifies the coefficient function K(X,Y):
%
%      function node_k = k_coef ( node_num, node_xy )
%
%    The user specifies the right hand side of the Poisson equation
%    by supplying a routine of the form
%
%      function node_rhs = rhs ( node_num, node_xy )
%
%  Usage:
%
%    fem2d_poisson_sparse ( 'prefix' )
%
%    where:
%
%    'prefix' is the common prefix for the node and element files:
%
%    * prefix_nodes.txt,     the node coordinates.
%    * prefix_elements.txt,  the nodes that make up each element.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Local parameters:
%
%    Local, real sparse A(:,), the finite element coefficient matrix.
%
%    Local, integer ELEMENT_NODE[3*ELEMENT_NUM];
%    ELEMENT_NODE(I,J) is the global index of local node I in element J.
%
%    Local, integer ELEMENT_NUM, the number of elements.
%
%    Local, integer ELEMENT_ORDER, the element order.
%
%    Local, real F(NODE_NUM,1), the right hand side.
%
%    Local, logical NODE_BOUNDARY(NODE_NUM), is TRUE if the node is
%    found to lie on the boundary of the region.
%
%    Local, integer NODE_CONDITION(NODE_NUM),
%    indicates the condition used to determine the variable at a node.
%    0, there is no condition (and no variable) at this node.
%    1, a finite element equation is used;
%    2, a Dirichlet condition is used.
%    3, a Neumann condition is used.
%
%    Local, integer NODE_NUM, the number of nodes.
%
%    Local, real NODE_R(NODE_NUM), the residual error.
%
%    Local, real NODE_U(NODE_NUM), the finite element coefficients.
%
%    Local, real NODE_XY(2,NODE_NUM), the coordinates of nodes.
%
%    Local, integer QUAD_NUM, the number of quadrature points used for
%    assembly.  This is currently set to 3, the lowest reasonable value.
%    Legal values are 1, 3, 4, 6, 7, 9, 13, and for some problems, a value
%    of QUAD_NUM greater than 3 may be appropriate.
%
  debugging = 0;
  quad_num = 3;

  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_POISSON_SPARSE:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A version of FEM2D_POISSON using MATLAB''s \n' );
  fprintf ( 1, '  sparse matrix storage, factor and solve facilities.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Finite element solution of the \n' );
  fprintf ( 1, '  steady Poisson equation on a triangulated region\n' );
  fprintf ( 1, '  in 2 dimensions.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  - DEL H(x,y) DEL U(x,y) + K(x,y) * U(x,y) = F(x,y) in the region\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                                     U(x,y) = G(x,y) on the boundary.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The finite element method is used,\n' );
  fprintf ( 1, '  with triangular elements,\n' );
  fprintf ( 1, '  which must be a 3 node linear triangle.\n' );
%
%  Must have prefix.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FEM2D_POISSON_SPARSE - Fatal error!\n' );
    fprintf ( 1, '  Missing "prefix", the common filename prefix input.\n' );
    error ( 'FEM2D_POISSON_SPARSE - Fatal error!\n' )
  end
%
%  Create the file names.
%
  node_filename = strcat ( prefix, '_nodes.txt' );
  element_filename = strcat ( prefix, '_elements.txt' );
  solution_filename = strcat ( prefix, '_values.txt' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node file is "%s".\n', node_filename );
  fprintf ( 1, '  Element file is "%s".\n', element_filename );
%
%  Read the node coordinate file.
%
  node_xy = load ( node_filename );
  node_xy = node_xy';
  [ dim_num, node_num ] = size ( node_xy );

  fprintf ( 1, '  Number of nodes =          %d\n', node_num );

  r8mat_transpose_print_some ( dim_num, node_num, node_xy, 1, 1, 2, 10, ...
    '  First 10 nodes' );
%
%  Read the element description file.
%
  element_node = load ( element_filename );
  element_node = element_node';
  [ element_order, element_num ] = size ( element_node );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Element order =            %d\n', element_order );
  fprintf ( 1, '  Number of elements =       %d\n', element_num );

  if ( element_order ~= 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FEM2D_POISSON_SPARSE - Fatal error!\n' );
    fprintf ( 1, '  The input triangulation has order %d.\n', element_order );
    fprintf ( 1, '  However, a triangulation of order 3 is required.' );
    error ( 'FEM2D_POISSON_SPARSE - Fatal error!' );
  end

  i4mat_transpose_print_some ( 3, element_num, ...
    element_node, 1, 1, 3, 10, '  First 10 elements' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature order =          %d\n', quad_num );
%
%  Determine which nodes are boundary nodes and which have a
%  finite element unknown.  Then set the boundary values.
%
  node_boundary = triangulation_order3_boundary_node ( node_num, ...
    element_num, element_node );
%
%  Determine the node conditions.
%  For now, we'll just assume all boundary nodes are Dirichlet.
%
  node_condition(1:node_num) = 1;

  for node = 1 : node_num
    if ( node_boundary(node) )
      node_condition(node) = 2;
    end
  end
%
%  Determine the element neighbor array, just so we can estimate
%  the nonzeros.
%
  element_neighbor = triangulation_neighbor_triangles ( ...
    element_order, element_num, element_node );
%
%  Determine the maximum number of nonzeros.
%
  [ nz_num, adj_col ] = triangulation_order3_adj_count ( node_num, ...
    element_num, element_node, element_neighbor );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_ADJ_COUNT returns NZ_NUM = %d\n', ...
    nz_num );
%
%  Assemble the finite element coefficient matrix A and the right-hand side F.
%
  [ a, f ] = assemble_poisson_sparse ( node_num, node_xy, ...
    element_num, element_node, quad_num, nz_num );

  if ( debugging )

    a_copy = full ( a );
    r8mat_print_some ( node_num, node_num, a_copy, 1, 1, 10, 10, ...
      '  Part of Poisson stiffness matrix A:' );

    r8vec_print_some ( node_num, f, 1, 10, ...
      '  Part of finite element right hand side vector F:' );

  end
%
%  Adjust the linear system to account for Dirichlet boundary conditions.
%
  [ a, f ] = dirichlet_apply_sparse ( node_num, node_xy, node_condition, ...
    a, f );

  if ( debugging )

    a_copy = full ( a );
    r8mat_print_some ( node_num, node_num, a_copy, 1, 1, 10, 10, ...
      '  Part of Matrix A after Dirichlet boundary adjustments:' );

    r8mat_print_some ( node_num, 1, f, 1, 1, 10, 1, ...
      '  Part of right hand side after Dirichlet boundary adjustments:' );

  end
%
%  Solve the linear system using MATLAB's sparse solver.
%
  node_u = a \ f;

  r8vec_print_some ( node_num, node_u, 1, 10, ...
    '  Part of the solution vector:' );

  node_r = residual_poisson ( node_num, node_xy, node_condition, ...
    element_num, element_node, quad_num, a, f, node_u );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Maximum absolute residual = %f\n',  ...
    max ( abs ( node_r(1:node_num) ) ) );
%
%  Write an ASCII file that can be read into MATLAB.
%
  save ( solution_filename, '-ascii', 'node_u' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_POISSON_SPARSE:\n' );
  fprintf ( 1, '  Wrote solution to the file "%s"\n', solution_filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_POISSON_SPARSE:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ a, f ] = assemble_poisson_sparse ( node_num, node_xy, ...
  element_num, element_node, quad_num, nz_num )

%*****************************************************************************80
%
%% ASSEMBLE_POISSON_SPARSE assembles the system for the Poisson equation.
%
%  Discussion:
%
%    The matrix is stored in MATLAB sparse matrix format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the coordinates of nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(3,ELEMENT_NUM);
%    element_NODE(I,J) is the global index of local node I in element J.
%
%    Input, integer QUAD_NUM, the number of quadrature points used in assembly.
%
%    Input, integer NZ_NUM, the (maximum) number of nonzeros in the matrix.
%    If set to 0 on input, we hope MATLAB's sparse utility will be able
%    to take over the task of reallocating space as necessary.
%
%    Output, real sparse A(:,:), the coefficient matrix.
%
%    Output, real F(NODE_NUM,1), the right hand side.
%
%  Local parameters:
%
%    Local, real BI, DBIDX, DBIDY, the value of some basis function
%    and its first derivatives at a quadrature point.
%
%    Local, real BJ, DBJDX, DBJDY, the value of another basis
%    function and its first derivatives at a quadrature point.
%

%
%  Initialize the arrays to zero.
%
  f(1:node_num,1) = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASSEMBLE_POISSON_SPARSE:\n' );
  fprintf ( 1, '  Setting up sparse Poisson matrix with NZ_NUM = %d\n', nz_num );

  a = sparse ( [], [], [], node_num, node_num, nz_num );
%
%  Get the quadrature weights and nodes.
%
  [ quad_w, quad_xy ] = quad_rule ( quad_num );
%
%  Add up all quantities associated with the element-th element.
%
  for element = 1 : element_num
%
%  Make a copy of the element.
%
    t3(1:2,1:3) = node_xy(1:2,element_node(1:3,element));
%
%  Map the quadrature points QUAD_XY to points PHYS_XY in the physical element.
%
    phys_xy(1:2,1:quad_num) = reference_to_physical_t3 ( t3, quad_num, quad_xy );

    area = abs ( triangle_area_2d ( t3 ) );

    w(1:quad_num,1) = quad_w(1:quad_num,1) * area;

    phys_rhs = rhs ( quad_num, phys_xy );
    phys_h = h_coef ( quad_num, phys_xy );
    phys_k = k_coef ( quad_num, phys_xy );
%
%  Consider the QUAD-th quadrature point..
%
    for quad = 1 : quad_num
%
%  Consider the TEST-th test function.
%
%  We generate an integral for every node associated with an unknown.
%  But if a node is associated with a boundary condition, we do nothing.
%
      for test = 1 : 3

        i = element_node(test,element);

        [ bi, dbidx, dbidy ] = basis_11_t3 ( t3, test, phys_xy(1:2,quad) );

        f(i,1) = f(i,1) + w(quad,1) * phys_rhs(quad,1) * bi;
%
%  Consider the BASIS-th basis function, which is used to form the
%  value of the solution function.
%
        for basis = 1 : 3

          j = element_node(basis,element);

          [ bj, dbjdx, dbjdy ] = basis_11_t3 ( t3, basis, phys_xy(1:2,quad) );

          a(i,j) = a(i,j) + w(quad,1) * ( ...
              phys_h(quad) * ( dbidx * dbjdx + dbidy * dbjdy ) ...
            + phys_k(quad) * bj * bi );

        end

      end

    end

  end

  return
end
function [ qi, dqidx, dqidy ] = basis_11_t3 ( t, i, p )

%*****************************************************************************80
%
%% BASIS_11_T3: one basis at one point for the T3 element.
%
%  Discussion:
%
%    The routine is given the coordinates of the nodes of a triangle.
%
%           3
%          / \
%         /   \
%        /     \
%       1-------2
%
%    It evaluates the linear basis function Q(I)(X,Y) associated with
%    node I, which has the property that it is a linear function
%    which is 1 at node I and zero at the other two nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the coordinates of the nodes.
%
%    Input, integer I, the index of the desired basis function.
%    I should be between 1 and 3.
%
%    Input, real P(2), the coordinates of a point at which the basis
%    function is to be evaluated.
%
%    Output, real QI, DQIDX, DQIDY, the values of the basis function
%    and its X and Y derivatives.
%
  area = abs ( t(1,1) * ( t(2,2) - t(2,3) ) ...
             + t(1,2) * ( t(2,3) - t(2,1) ) ...
             + t(1,3) * ( t(2,1) - t(2,2) ) );

  if ( area == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASIS_11_T3 - Fatal error!\n' );
    fprintf ( 1, '  Element has zero area.\n' );
    error ( 'BASIS_11_T3 - Fatal error!' );
  end

  if ( i < 1 | 3 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASIS_11_T3 - Fatal error!\n' );
    fprintf ( 1, '  Basis index I is not between 1 and 3.\n' );
    fprintf ( 1, '  I = %d\n', i );
    error ( 'BASIS_11_T3 - Fatal error!' );
  end

  if ( i == 1 )
    ip1 = 2;
    ip2 = 3;
  elseif ( i == 2 )
    ip1 = 3;
    ip2 = 1;
  else
    ip1 = 1;
    ip2 = 2;
  end

  qi = ( ( t(1,ip2) - t(1,ip1) ) * ( p(2) - t(2,ip1) ) ...
       - ( t(2,ip2) - t(2,ip1) ) * ( p(1) - t(1,ip1) ) ) / area;

  dqidx = - ( t(2,ip2) - t(2,ip1) ) / area;
  dqidy =   ( t(1,ip2) - t(1,ip1) ) / area;

  return
end
function [ a, f ] = dirichlet_apply_sparse ( node_num, node_xy, ...
  node_condition, a, f )

%*****************************************************************************80
%
%% DIRICHLET_APPLY_SPARSE accounts for Dirichlet boundary conditions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the coordinates of nodes.
%
%    Input, integer NODE_CONDITION(NODE_NUM), reports the condition
%    used to set the unknown associated with the node.
%    0, unknown.
%    1, finite element equation.
%    2, Dirichlet condition;
%    3, Neumann condition.
%
%    Input, real sparse A(:,:), the coefficient matrix.
%
%    Input, real F(NODE_NUM,1), the right hand side.
%
%    Output, real sparse A(:,:), the coefficient matrix,
%    adjusted for Dirichlet boundary conditions.
%
%    Output, real F(NODE_NUM), the right hand side, adjusted for
%    Dirichlet boundary conditions.
%
  node_bc = dirichlet_condition ( node_num, node_xy );

  DIRICHLET = 2;

  for node = 1 : node_num

    if ( node_condition(node) == DIRICHLET )

      a(node,:) = 0.0;
      a(node,node) = 1.0;

      f(node,1) = node_bc(node);

    end

  end

  return
end
function isgn = i4col_compare ( m, n, a, i, j )

%*****************************************************************************80
%
%% I4COL_COMPARE compares columns I and J of a integer array.
%
%  Example:
%
%    Input:
%
%      M = 3, N = 4, I = 2, J = 4
%
%      A = (
%        1  2  3  4
%        5  6  7  8
%        9 10 11 12 )
%
%    Output:
%
%      ISGN = -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an array of N columns of vectors of length M.
%
%    Input, integer I, J, the columns to be compared.
%    I and J must be between 1 and N.
%
%    Output, integer ISGN, the results of the comparison:
%    -1, column I < column J,
%     0, column I = column J,
%    +1, column J < column I.
%

%
%  Check.
%
  if ( i < 1)
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  Column index I = %d < 1.\n', i );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  if ( n < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  N = %d < column index I = %d.\n', n, i );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  if ( j < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  Column index J = %d < 1.\n', j );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  if ( n < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  N = %d < column index J = %d.\n', n, j );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  isgn = 0;

  if ( i == j )
    return
  end

  k = 1;

  while ( k <= m )

    if ( a(k,i) < a(k,j) )
      isgn = -1;
      return
    elseif ( a(k,j) < a(k,i) )
      isgn = +1;
      return
    end

    k = k + 1;

  end

  return
end
function a = i4col_sort_a ( m, n, a )

%*****************************************************************************80
%
%% I4COL_SORT_A ascending sorts an I4COL.
%
%  Discussion:
%
%    In lexicographic order, the statement "X < Y", applied to two real
%    vectors X and Y of length M, means that there is some index I, with
%    1 <= I <= M, with the property that
%
%      X(J) = Y(J) for J < I,
%    and
%      X(I) < Y(I).
%
%    In other words, the first time they differ, X is smaller.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows of A, and the length of
%    a vector of data.
%
%    Input, integer N, the number of columns of A.
%
%    Input, integer A(M,N), the array of N columns of M-vectors.
%
%    Output, integer A(M,N), the columns of A have been sorted in ascending
%    lexicographic order.
%
  if ( m <= 0 )
    return
  end

  if ( n <= 1 )
    return
  end
%
%  Initialize.
%
  i = 0;
  indx = 0;
  isgn = 0;
  j = 0;
%
%  Call the external heap sorter.
%
  while ( 1 )

    [ indx, i, j ] = sort_heap_external ( n, indx, isgn );
%
%  Interchange the I and J objects.
%
    if ( 0 < indx )

      a = i4col_swap ( m, n, a, i, j );
%
%  Compare the I and J objects.
%
    elseif ( indx < 0 )

      isgn = i4col_compare ( m, n, a, i, j );

    elseif ( indx == 0 )

      break

    end

  end

  return
end
function a = i4col_swap ( m, n, a, i, j )

%*****************************************************************************80
%
%% I4COL_SWAP swaps columns I and J of a integer array of column data.
%
%  Example:
%
%    Input:
%
%      M = 3, N = 4, I = 2, J = 4
%
%      A = (
%        1  2  3  4
%        5  6  7  8
%        9 10 11 12 )
%
%    Output:
%
%      A = (
%        1  4  3  2
%        5  8  7  6
%        9 12 11 10 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, integer A(M,N), an array of N columns of length M.
%
%    Input, integer I, J, the columns to be swapped.
%
%    Output, integer A(M,N), the array, with columns I and J swapped.
%
  if ( i < 1 | n < i | j < 1 | n < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_SWAP - Fatal error!\n' );
    fprintf ( 1, '  I or J is out of bounds.\n' );
    fprintf ( 1, '  I =    %d\n', i );
    fprintf ( 1, '  J =    %d\n', j );
    fprintf ( 1, '  N =    %d\n', n );
    error ( 'I4COL_SWAP - Fatal error!' );
  end

  if ( i == j )
    return
  end

  col(1:m) = a(1:m,i)';
  a(1:m,i) = a(1:m,j);
  a(1:m,j) = col(1:m)';

  return
end
function i4mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% I4MAT_TRANSPOSE_PRINT_SOME prints some of an I4MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, a title.
%
  incx = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d  ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );
    fprintf ( 1, '\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d  ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%7d  ', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function [ quad_w, quad_xy ] = quad_rule ( quad_num )

%*****************************************************************************80
%
%% QUAD_RULE sets the quadrature rule for assembly.
%
%  Discussion:
%
%    The quadrature rule is given for a reference element, points (X,Y) such
%    that
%
%      0 <= X,
%      0 <= Y, and
%      X + Y <= 1.
%
%      ^
%    1 | *
%      | |\
%    Y | | \
%      | |  \
%    0 | *---*
%      +------->
%        0 X 1
%
%    The rules have the following precision:
%
%    QUAD_NUM  Precision
%
%     1        1
%     3        2
%     4        3
%     6        4
%     7        5
%     9        6
%    13        7
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer QUAD_NUM, the number of quadrature nodes.
%    Legal values are 1, 3, 4, 6, 7, 9, 13.
%
%    Output, real QUAD_W(QUAD_NUM,1), the quadrature weights.
%
%    Output, real QUAD_XY(2,QUAD_NUM), the quadrature nodes.
%
  if ( quad_num == 1 )

    quad_xy(1:2,1:quad_num) = [ 1.0 / 3.0, 1.0 / 3.0 ]';

    quad_w(1:quad_num,1) = 1.0;

  elseif ( quad_num == 3 )

    quad_xy(1:2,1:quad_num) = [ ...
      0.5, 0.0; ...
      0.5, 0.5; ...
      0.0, 0.5 ]';

    quad_w(1:quad_num,1) = 1.0 / 3.0;

  elseif ( quad_num == 4 )

    a =   6.0;
    b =  10.0;
    c =  18.0;
    d =  25.0;
    e = -27.0;
    f =  30.0;
    g =  48.0;

    quad_xy(1:2,1:quad_num) = [ ...
      b, b; ...
      c, a; ...
      a, c; ...
      a, a ]' / f;

    quad_w(1:quad_num,1) = [ e, d, d, d ]' / g;

  elseif ( quad_num == 6 )

    a = 0.816847572980459;
    b = 0.091576213509771;
    c = 0.108103018168070;
    d = 0.445948490915965;
    v = 0.109951743655322;
    w = 0.223381589678011;

    quad_xy(1:2,1:quad_num) = [
      a, b; ...
      b, a; ...
      b, b; ...
      c, d; ...
      d, c; ...
      d, d ]';

    quad_w(1:6,1) = [ v, v, v, w, w, w ]';

  elseif ( quad_num == 7 )

    a = 1.0 / 3.0;
    b = ( 9.0 + 2.0 * sqrt ( 15.0 ) ) / 21.0;
    c = ( 6.0 -       sqrt ( 15.0 ) ) / 21.0;
    d = ( 9.0 - 2.0 * sqrt ( 15.0 ) ) / 21.0;
    e = ( 6.0 +       sqrt ( 15.0 ) ) / 21.0;
    u = 0.225;
    v = ( 155.0 - sqrt ( 15.0 ) ) / 1200.0;
    w = ( 155.0 + sqrt ( 15.0 ) ) / 1200.0;

    quad_xy(1:2,1:quad_num) = [ ...
      a, a; ...
      b, c; ...
      c, b; ...
      c, c; ...
      d, e; ...
      e, d; ...
      e, e ]';

    quad_w(1:quad_num,1) = [ u, v, v, v, w, w, w ]';

  elseif ( quad_num == 9 )

    a = 0.124949503233232;
    b = 0.437525248383384;
    c = 0.797112651860071;
    d = 0.165409927389841;
    e = 0.037477420750088;

    u = 0.205950504760887;
    v = 0.063691414286223;

    quad_xy(1:2,1:quad_num) = [ ...
      a, b; ...
      b, a; ...
      b, b; ...
      c, d; ...
      c, e; ...
      d, c; ...
      d, e; ...
      e, c; ...
      e, d ]';

    quad_w(1:quad_num,1) = [ u, u, u, v, v, v, v, v, v ]';

  elseif ( quad_num == 13 )

    h = 1.0 / 3.0;
    a = 0.479308067841923;
    b = 0.260345966079038;
    c = 0.869739794195568;
    d = 0.065130102902216;
    e = 0.638444188569809;
    f = 0.312865496004875;
    g = 0.048690315425316;

    w = -0.149570044467670;
    t =  0.175615257433204;
    u =  0.053347235608839;
    v =  0.077113760890257;

    quad_xy(1:2,1:quad_num) = [
      h, h; ...
      a, b; ...
      b, a; ...
      b, b; ...
      c, d; ...
      d, c; ...
      d, d; ...
      e, f; ...
      e, g; ...
      f, e; ...
      f, g; ...
      g, e; ...
      g, f ]';

    quad_w(1:quad_num,1) = [ w, t, t, t, u, u, u, v, v, v, v, v, v ]';

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'QUAD_RULE - Fatal error!\n' );
    fprintf ( 1, '  No rule is available of order QUAD_NUM = %d\n', ...
      quad_num );
    error ( 'QUAD_RULE - Fatal error!\n' );

  end

  return
end
function r8mat_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_PRINT_SOME prints out a portion of an R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, a title.
%
  incx = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  for j2lo = max ( jlo, 1 ): incx : min ( jhi, n )

    j2hi = j2lo + incx - 1;
    j2hi = min ( j2hi, n );
    j2hi = min ( j2hi, jhi );

    inc = j2hi + 1 - j2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col: ' );

    for j = j2lo : j2hi
      fprintf ( 1, '%7d       ', j );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row\n' );

    i2lo = max ( ilo, 1 );
    i2hi = min ( ihi, m );

    for i = i2lo : i2hi

      fprintf ( 1, '%7d  ', i );

      for j = j2lo : j2hi
        fprintf ( 1, '%12g  ', a(i,j) );
      end

      fprintf ( 1, '\n' );

    end

  end

  return
end
function r8mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_SOME prints some of an R8MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, a title.
%
  incx = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d       ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%12f', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function r8vec_print_some ( n, a, i_lo, i_hi, title )

%*****************************************************************************80
%
%% R8VEC_PRINT_SOME prints "some" of an R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8 values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%
%    Input, real A(N), the vector to be printed.
%
%    Input, integer MAX_PRINT, the maximum number of lines to print.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );
  fprintf ( 1, '\n' );

  for i = max ( 1, i_lo ) : min ( n, i_hi )
    fprintf ( 1, '  %8d  %12f\n', i, a(i) );
  end

  return
end
function phy = reference_to_physical_t3 ( t, n, ref )

%*****************************************************************************80
%
%% REFERENCE_TO_PHYSICAL_T3 maps a reference point to a physical point.
%
%  Discussion:
%
%    Given the vertices of an order 3 physical triangle and a point
%    (XSI,ETA) in the reference triangle, the routine computes the value
%    of the corresponding image point (X,Y) in physical space.
%
%    Note that this routine may also be appropriate for an order 6
%    triangle, if the mapping between reference and physical space
%    is linear.  This implies, in particular, that the sides of the
%    image triangle are straight and that the "midside" nodes in the
%    physical triangle are literally halfway along the sides of
%    the physical triangle.
%
%  Reference Element T3:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the coordinates of the vertices.  The vertices are assumed
%    to be the images of (0,0), (1,0) and (0,1) respectively.
%
%    Input, integer N, the number of objects to transform.
%
%    Input, real REF(2,N), the coordinates of points in the reference space.
%
%    Output, real PHY(2,N), the coordinates of the corresponding points in the
%    physical space.
%
  for i = 1 : 2

    phy(i,1:n) = t(i,1) * ( 1.0 - ref(1,1:n) - ref(2,1:n) ) ...
               + t(i,2) *        ref(1,1:n)                ...
               + t(i,3) *                     ref(2,1:n);
  end

  return
end
function node_r = residual_poisson ( node_num, node_xy, node_condition, ...
  element_num, element_node, quad_num, a, f, node_u )

%*****************************************************************************80
%
%% RESIDUAL_POISSON evaluates the residual for the Poisson equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the
%    coordinates of nodes.
%
%    Input, integer NODE_CONDITION(NODE_NUM), reports the condition
%    used to set the unknown associated with the node.
%    0, unknown.
%    1, finite element equation.
%    2, Dirichlet condition;
%    3, Neumann condition.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(3,ELEMENT_NUM);
%    ELEMENT_NODE(I,J) is the global index of local node I in element J.
%
%    Input, integer QUAD_NUM, the number of quadrature points used in assembly.
%
%    Input, integer INDX(NODE_NUM), gives the index of the unknown quantity
%    associated with the given node.
%
%    Workspace, real sparse A(:,:), the NODE_NUM by NODE_NUM
%    coefficient matrix, stored in MATLAB sparse format.
%
%    Workspace, real F(NODE_NUM,1), the right hand side.
%
%    Input, real NODE_U(NODE_NUM), the value of the solution
%    at each node.
%
%    Output, real NODE_R(NODE_NUM), the finite element
%    residual at each node.
%
%  Local parameters:
%
%    Local, real BI, DBIDX, DBIDY, the value of some basis function
%    and its first derivatives at a quadrature point.
%
%    Local, real BJ, DBJDX, DBJDY, the value of another basis
%    function and its first derivatives at a quadrature point.
%

%
%  Initialize the arrays to zero.
%
  f(1:node_num,1) = 0.0;
  a(:,:) = 0.0;
%
%  Get the quadrature weights and nodes.
%
  [ quad_w, quad_xy ] = quad_rule ( quad_num );
%
%  The actual values of A and F are determined by summing up
%  contributions from all the elements.
%
  for element = 1 : element_num
%
%  Make a copy of the element.
%
    t3(1:2,1:3) = node_xy(1:2,element_node(1:3,element));
%
%  Map the quadrature points QUAD_XY to points PHYS_XY in the physical element.
%
    phys_xy(1:2,1:quad_num) = reference_to_physical_t3 ( t3, quad_num, quad_xy );
    area = abs ( triangle_area_2d ( t3 ) );
    w(1:quad_num) = area * quad_w(1:quad_num);

    phys_rhs = rhs ( quad_num, phys_xy );
    phys_h = h_coef ( quad_num, phys_xy );
    phys_k = k_coef ( quad_num, phys_xy );
%
%  Consider a quadrature point QUAD, with coordinates (X,Y).
%
    for quad = 1 : quad_num
%
%  Consider one of the basis functions, which will play the
%  role of test function in the integral.
%
%  We generate an integral for every node associated with an unknown.
%  But if a node is associated with a boundary condition, we do nothing.
%
      for test = 1 : 3

        i = element_node(test,element);

        [ bi, dbidx, dbidy ] = basis_11_t3 ( t3, test, phys_xy(1:2,quad) );

        f(i,1) = f(i,1) + w(quad) * phys_rhs(quad) * bi;
%
%  Consider another basis function, which is used to form the
%  value of the solution function.
%
        for basis = 1 : 3

          j = element_node(basis,element);

          [ bj, dbjdx, dbjdy ] = basis_11_t3 ( t3, basis, phys_xy(1:2,quad) );

          a(i,j) = a(i,j) + w(quad) * ( ...
              phys_h(quad) * ( dbidx * dbjdx + dbidy * dbjdy ) ...
            + phys_k(quad) * bj * bi );

        end

      end

    end

  end
%
%  Apply boundary conditions.
%
  [ a, f ] = dirichlet_apply_sparse ( node_num, node_xy, node_condition, a, f );
%
%  Compute A*U.
%
  node_r = a * node_u;
%
%  Set RES = A * U - F.
%
  node_r(1:node_num) = node_r(1:node_num) - f(1:node_num,1);

  return
end
function [ indx, i, j ] = sort_heap_external ( n, indx, isgn )

%*****************************************************************************80
%
%% SORT_HEAP_EXTERNAL externally sorts a list of items into ascending order.
%
%  Discussion:
%
%    The actual list of data is not passed to the routine.  Hence this
%    routine may be used to sort integers, reals, numbers, names,
%    dates, shoe sizes, and so on.  After each call, the routine asks
%    the user to compare or interchange two items, until a special
%    return value signals that the sorting is completed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis, Herbert Wilf.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf.
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the number of items to be sorted.
%
%    Input, integer INDX, the main communication signal.
%    The user must set INDX to 0 before the first call.
%    Thereafter, the user should set the input value of INDX
%    to the output value from the previous call.
%
%    Input, integer ISGN, results of comparison of elements I and J.
%    (Used only when the previous call returned INDX less than 0).
%    ISGN <= 0 means I is less than or equal to J;
%    0 <= ISGN means I is greater than or equal to J.
%
%    Output, integer INDX, the main communication signal.
%    If INDX is
%
%      greater than 0, the user should:
%      * interchange items I and J;
%      * call again.
%
%      less than 0, the user should:
%      * compare items I and J;
%      * set ISGN = -1 if I < J, ISGN = +1 if J < I;
%      * call again.
%
%      equal to 0, the sorting is done.
%
%    Output, integer I, J, the indices of two items.
%    On return with INDX positive, elements I and J should be interchanged.
%    On return with INDX negative, elements I and J should be compared, and
%    the result reported in ISGN on the next call.
%
  persistent i_save;
  persistent j_save;
  persistent k;
  persistent k1;
  persistent n1;

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

  if ( isempty ( j_save ) )
    j_save = -1;
  end
%
%  INDX = 0: This is the first call.
%
  if ( indx == 0 )

    k = floor ( n / 2 );
    k1 = k;
    n1 = n;
%
%  INDX < 0: The user is returning the results of a comparison.
%
  elseif ( indx < 0 )

    if ( indx == -2 )

      if ( isgn < 0 )
        i_save = i_save + 1;
      end

      j_save = k1;
      k1 = i_save;
      indx = -1;
      i = i_save;
      j = j_save;
      return;
    end

    if ( 0 < isgn )
      indx = 2;
      i = i_save;
      j = j_save;
      return;
    end

    if ( k <= 1 )

      if ( n1 == 1 )
        i_save = 0;
        j_save = 0;
        indx = 0;
      else
        i_save = n1;
        n1 = n1 - 1;
        j_save = 1;
        indx = 1;
      end

      i = i_save;
      j = j_save;
      return;

    end

    k = k - 1;
    k1 = k;
%
%  0 < INDX, the user was asked to make an interchange.
%
  elseif ( indx == 1 )

    k1 = k;

  end

  while ( 1 )

    i_save = 2 * k1;

    if ( i_save == n1 )
      j_save = k1;
      k1 = i_save;
      indx = -1;
      i = i_save;
      j = j_save;
      return;
    elseif ( i_save <= n1 )
      j_save = i_save + 1;
      indx = -2;
      i = i_save;
      j = j_save;
      return;
    end

    if ( k <= 1 )
      break;
    end

    k = k - 1;
    k1 = k;

  end

  if ( n1 == 1 )
    i_save = 0;
    j_save = 0;
    indx = 0;
    i = i_save;
    j = j_save;
  else
    i_save = n1;
    n1 = n1 - 1;
    j_save = 1;
    indx = 1;
    i = i_save;
    j = j_save;
  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end
function area = triangle_area_2d ( t )

%*****************************************************************************80
%
%% TRIANGLE_AREA_2D computes the area of a triangle in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, real AREA, the absolute area of the triangle.
%
  area = 0.5 * abs ( ...
      t(1,1) * ( t(2,2) - t(2,3) ) ...
    + t(1,2) * ( t(2,3) - t(2,1) ) ...
    + t(1,3) * ( t(2,1) - t(2,2) ) );

  return
end
function triangle_neighbor = triangulation_neighbor_triangles ( ...
  triangle_order, triangle_num, triangle_node )

%*****************************************************************************80
%
%% TRIANGULATION_NEIGHBOR_TRIANGLES determines triangle neighbors.
%
%  Discussion:
%
%    A triangulation of a set of nodes can be completely described by
%    the coordinates of the nodes, and the list of nodes that make up
%    each triangle.  However, in some cases, it is necessary to know
%    triangle adjacency information, that is, which triangle, if any,
%    is adjacent to a given triangle on a particular side.
%
%    This routine creates a data structure recording this information.
%
%    The primary amount of work occurs in sorting a list of 3 * TRIANGLE_NUM
%    data items.
%
%    This routine was modified to use columns instead of rows.
%
%  Example:
%
%    The input information from TRIANGLE_NODE:
%
%    Triangle   Nodes
%    --------   ---------------
%     1         3      4      1
%     2         3      1      2
%     3         3      2      8
%     4         2      1      5
%     5         8      2     13
%     6         8     13      9
%     7         3      8      9
%     8        13      2      5
%     9         9     13      7
%    10         7     13      5
%    11         6      7      5
%    12         9      7      6
%    13        10      9      6
%    14         6      5     12
%    15        11      6     12
%    16        10      6     11
%
%    The output information in TRIANGLE_NEIGHBOR:
%
%    Triangle  Neighboring Triangles
%    --------  ---------------------
%
%     1        -1     -1      2
%     2         1      4      3
%     3         2      5      7
%     4         2     -1      8
%     5         3      8      6
%     6         5      9      7
%     7         3      6     -1
%     8         5      4     10
%     9         6     10     12
%    10         9      8     11
%    11        12     10     14
%    12         9     11     13
%    13        -1     12     16
%    14        11     -1     15
%    15        16     14     -1
%    16        13     15     -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TRIANGLE_ORDER, the order of the triangles.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(TRIANGLE_ORDER,TRIANGLE_NUM), the nodes that
%    make up each triangle.
%
%    Output, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the three triangles
%    that are direct neighbors of a given triangle.  TRIANGLE_NEIGHBOR(1,I) is
%    the index of the triangle which touches side 1, defined by nodes 2 and 3,
%    and so on.  TRIANGLE_NEIGHBOR(1,I) is negative if there is no neighbor
%    on that side.  In this case, that side of the triangle lies on the
%    boundary of the triangulation.
%

%
%  Step 1.
%  From the list of nodes for triangle T, of the form: (I,J,K)
%  construct the three neighbor relations:
%
%    (I,J,3,T) or (J,I,3,T),
%    (J,K,1,T) or (K,J,1,T),
%    (K,I,2,T) or (I,K,2,T)
%
%  where we choose (I,J,3,T) if I < J, or else (J,I,3,T)
%
  col = zeros ( 4, triangle_order * triangle_num );

  for tri = 1 : triangle_num

    i = triangle_node(1,tri);
    j = triangle_node(2,tri);
    k = triangle_node(3,tri);

    if ( i < j )
      col(1:4,1+3*(tri-1)) = [ i, j, 3, tri ]';
    else
      col(1:4,1+3*(tri-1)) = [ j, i, 3, tri ]';
    end

    if ( j < k )
      col(1:4,2+3*(tri-1)) = [ j, k, 1, tri ]';
    else
      col(1:4,2+3*(tri-1)) = [ k, j, 1, tri ]';
    end

    if ( k < i )
      col(1:4,3+3*(tri-1)) = [ k, i, 2, tri ]';
    else
      col(1:4,3+3*(tri-1)) = [ i, k, 2, tri ]';
    end

  end
%
%  Step 2. Perform an ascending dictionary sort on the neighbor relations.
%  We only intend to sort on rows 1 and 2; the routine we call here
%  sorts on rows 1 through 4 but that won't hurt us.
%
%  What we need is to find cases where two triangles share an edge.
%  Say they share an edge defined by the nodes I and J.  Then there are
%  two columns of COL that start out ( I, J, ?, ? ).  By sorting COL,
%  we make sure that these two columns occur consecutively.  That will
%  make it easy to notice that the triangles are neighbors.
%
  col = i4col_sort_a ( 4, 3*triangle_num, col );
%
%  Step 3. Neighboring triangles show up as consecutive columns with
%  identical first two entries.  Whenever you spot this happening,
%  make the appropriate entries in TRIANGLE_NEIGHBOR.
%
  triangle_neighbor(1:3,1:triangle_num) = -1;

  icol = 1;

  while ( 1 )

    if ( 3 * triangle_num <= icol )
      break
    end

    if ( col(1,icol) ~= col(1,icol+1) || col(2,icol) ~= col(2,icol+1) )
      icol = icol + 1;
      continue
    end

    side1 = col(3,icol);
    tri1 =  col(4,icol);
    side2 = col(3,icol+1);
    tri2 =  col(4,icol+1);

    triangle_neighbor(side1,tri1) = tri2;
    triangle_neighbor(side2,tri2) = tri1;

    icol = icol + 2;

  end

  return
end
function [ adj_num, adj_col ] = triangulation_order3_adj_count ( node_num, ...
  tri_num, triangle_node, triangle_neighbor )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_ADJ_COUNT counts adjacencies in a triangulation.
%
%  Discussion:
%
%    This routine is called to count the adjacencies, so that the
%    appropriate amount of memory can be set aside for storage when
%    the adjacency structure is created.
%
%    The triangulation is assumed to involve 3-node triangles.
%
%    Two nodes are "adjacent" if they are both nodes in some triangle.
%    Also, a node is considered to be adjacent to itself.
%
%  Diagram:
%
%       3
%    s  |\
%    i  | \
%    d  |  \
%    e  |   \  side 2
%       |    \
%    3  |     \
%       |      \
%       1-------2
%
%         side 1
%
%    The local node numbering
%
%
%   21-22-23-24-25
%    |\ |\ |\ |\ |
%    | \| \| \| \|
%   16-17-18-19-20
%    |\ |\ |\ |\ |
%    | \| \| \| \|
%   11-12-13-14-15
%    |\ |\ |\ |\ |
%    | \| \| \| \|
%    6--7--8--9-10
%    |\ |\ |\ |\ |
%    | \| \| \| \|
%    1--2--3--4--5
%
%    A sample grid.
%
%
%    Below, we have a chart that summarizes the adjacency relationships
%    in the sample grid.  On the left, we list the node, and its neighbors,
%    with an asterisk to indicate the adjacency of the node to itself
%    (in some cases, you want to count this self adjacency and in some
%    you don't).  On the right, we list the number of adjancencies to
%    lower-indexed nodes, to the node itself, to higher-indexed nodes,
%    the total number of adjacencies for this node, and the location
%    of the first and last entries required to list this set of adjacencies
%    in a single list of all the adjacencies.
%
%    N   Adjacencies                Below  Self   Above   Total First  Last
%
%   --  -- -- -- -- -- -- --           --    --      --      --   ---     0
%    1:  *  2  6                        0     1       2       3     1     3
%    2:  1  *  3  6  7                  1     1       3       5     4     8
%    3:  2  *  4  7  8                  1     1       3       5     9    13
%    4:  3  *  5  8  9                  1     1       3       5    14    18
%    5:  4  *  9 10                     1     1       2       4    19    22
%    6:  1  2  *  7 11                  2     1       2       5    23    27
%    7:  2  3  6  *  8 11 12            3     1       3       7    28    34
%    8:  3  4  7  *  9 12 13            3     1       3       7    35    41
%    9:  4  5  8  * 10 13 14            3     1       3       7    42    48
%   10:  5  9  * 14 15                  2     1       2       5    49    53
%   11:  6  7  * 12 16                  2     1       2       5    54    58
%   12:  7  8 11  * 13 16 17            3     1       3       7    59    65
%   13:  8  9 12  * 14 17 18            3     1       3       7    66    72
%   14:  9 10 13  * 15 18 19            3     1       3       7    73    79
%   15: 10 14  * 19 20                  2     1       2       5    80    84
%   16: 11 12  * 17 21                  2     1       2       5    85    89
%   17: 12 13 16  * 18 21 22            3     1       3       7    90    96
%   18: 13 14 17  * 19 22 23            3     1       3       7    97   103
%   19: 14 15 18  * 20 23 24            3     1       3       7   104   110
%   20: 15 19  * 24 25                  2     1       2       5   111   115
%   21: 16 17  * 22                     2     1       1       4   116   119
%   22: 17 18 21  * 23                  3     1       1       5   120   124
%   23: 18 19 22  * 24                  3     1       1       5   125   129
%   24: 19 20 23  * 25                  3     1       1       5   130   134
%   25: 20 24  *                        2     1       0       3   135   137
%   --  -- -- -- -- -- -- --           --    --      --      --   138   ---
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TRI_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRI_NUM), lists the nodes that
%    make up each triangle, in counterclockwise order.
%
%    Input, integer TRIANGLE_NEIGHBOR(3,TRI_NUM), for each side of
%    a triangle, lists the neighboring triangle, or -1 if there is
%    no neighbor.
%
%    Output, integer ADJ_NUM, the number of adjacencies.
%
%    Output, integer ADJ_COL(NODE_NUM+1).  Information about column J is stored
%    in entries ADJ_COL(J) through ADJ_COL(J+1)-1 of ADJ.
%
  triangle_order = 3;
  adj_num = 0;
%
%  Set every node to be adjacent to itself.
%
  adj_col(1:node_num) = 1;
%
%  Examine each triangle.
%
  for triangle = 1 : tri_num

    n1 = triangle_node(1,triangle);
    n2 = triangle_node(2,triangle);
    n3 = triangle_node(3,triangle);
%
%  Add edge (1,2) if this is the first occurrence,
%  that is, if the edge (1,2) is on a boundary (TRIANGLE2 <= 0)
%  or if this triangle is the first of the pair in which the edge
%  occurs (TRIANGLE < TRIANGLE2).
%
    triangle2 = triangle_neighbor(1,triangle);

    if ( triangle2 < 0 | triangle < triangle2 )
      adj_col(n1) = adj_col(n1) + 1;
      adj_col(n2) = adj_col(n2) + 1;
    end
%
%  Add edge (2,3).
%
    triangle2 = triangle_neighbor(2,triangle);

    if ( triangle2 < 0 | triangle < triangle2 )
      adj_col(n2) = adj_col(n2) + 1;
      adj_col(n3) = adj_col(n3) + 1;
    end
%
%  Add edge (3,1).
%
    triangle2 = triangle_neighbor(3,triangle);

    if ( triangle2 < 0 | triangle < triangle2 )
      adj_col(n1) = adj_col(n1) + 1;
      adj_col(n3) = adj_col(n3) + 1;
    end

  end
%
%  We used ADJ_COL to count the number of entries in each column.
%  Convert it to pointers into the ADJ array.
%
  adj_col(2:node_num+1) = adj_col(1:node_num);

  adj_col(1) = 1;
  for i = 2 : node_num+1
    adj_col(i) = adj_col(i-1) + adj_col(i);
  end

  adj_num = adj_col(node_num+1) - 1;

  return
end
function node_boundary = triangulation_order3_boundary_node ( node_num, ...
  triangle_num, triangle_node )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_BOUNDARY_NODE indicates which nodes are on the boundary.
%
%  Discussion:
%
%    This routine is given a triangulation, an abstract list of sets of
%    of nodes.  It is assumed that the nodes in each triangle are listed
%    in a counterclockwise order, although the routine should work
%    if the nodes are consistently listed in a clockwise order as well.
%
%    It is assumed that each edge of the triangulation is either
%    * an INTERIOR edge, which is listed twice, once with positive
%      orientation and once with negative orientation, or;
%    * a BOUNDARY edge, which will occur only once.
%
%    This routine should work even if the region has holes - as long
%    as the boundary of the hole comprises more than 3 edges!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRIANGLE_NUM),
%    the nodes that make up the triangles.  These should be listed
%    in counterclockwise order.
%
%    Output, logical NODE_BOUNDARY(NODE_NUM), is TRUE if the node
%    is on a boundary edge.
%
  m = 2;
  n = 3 * triangle_num;
%
%  Set up the edge array.
%
  edge(1:2,               1:  triangle_num) = triangle_node(1:2,1:triangle_num);
  edge(1:2,  triangle_num+1:2*triangle_num) = triangle_node(2:3,1:triangle_num);
  edge(1,  2*triangle_num+1:3*triangle_num) = triangle_node(3,  1:triangle_num);
  edge(2,  2*triangle_num+1:3*triangle_num) = triangle_node(1,  1:triangle_num);
%
%  In each column, force the smaller entry to appear first.
%
  e1(1:n) = min ( edge(1:2,1:n) );
  e2(1:n) = max ( edge(1:2,1:n) );

  edge(1,1:n) = e1(1:n);
  edge(2,1:n) = e2(1:n);
%
%  Ascending sort the column array.
%
  edge = i4col_sort_a ( m, n, edge );
%
%  Records which appear twice are internal edges and can be ignored.
%
  node_boundary(1:node_num) = 0;

  i = 0;

  while ( i < 3 * triangle_num )

    i = i + 1;

    if ( i == 3 * triangle_num )
      node_boundary(edge(1:m,i)) = 1;
    elseif ( all ( edge(1:m,i) == edge(1:m,i+1) ) )
      i = i + 1;
    else
      node_boundary(edge(1:m,i)) = 1;
    end

  end

  return
end


function geometry_test20701 ( )

%*****************************************************************************80
%
%% TEST20701 tests TRIANGLE_INRADIUS_2D;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;

  t = [ ...
    0.0, 1.0; ...
    0.0, 0.0; ...
    1.0, 0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST20701\n' );
  fprintf ( 1, '  For a triangle in 2D,\n' );
  fprintf ( 1, '  TRIANGLE_INRADIUS_2D computes the inradius.\n' );

  r8mat_transpose_print ( dim_num, 3, t, '  Triangle vertices:' );

  r = triangle_inradius_2d ( t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Incircle radius is %f\n', r );

  return
end


%KSOBEL Sobel edge detector
%
% K = KSOBEL() is the Sobel x-derivative kernel:
%         1/8  |1  0  -1|
%              |2  0  -2|
%              |1  0  -1|
%
% Notes::
% - This kernel is an effective vertical-edge detector
% - The y-derivative (horizontal-edge) kernel is K'
%
% See also ISOBEL.

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

function k = ksobel

    k = [   1 0 -1
            2 0 -2
            1 0 -1] / 8;


function Corr = tapas_Cov2Corr(Cov)
% Converts a covariance matrix into a correlation matrix
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Check if Cov is symmetric
if any(any(Cov'~=Cov))
    error('tapas:hgf:Cov2Corr:MatNotSymm', 'Input matrix is not symmetric.');
end

% Check if Cov is positive semi-definite
if any(isinf(Cov(:))) || any(isnan(Cov(:))) || any(eig(Cov)<0)
    error('tapas:hgf:Cov2Corr:MatNotPosDef', 'Input matrix is not positive semi-definite.');
end

sdev = sqrt(diag(Cov));
Norm = sdev * sdev';
Corr = Cov./Norm;

return;


function [Xrls,yrls,s]=RLSreg(y,X)
%Syntax: [Xrls,yrls,s]=RLSreg(y,X)
%_______________________________
%
% Calculates the Reweighted Least Squares (RLS) regression data points
% and the scale parameter from the LMS regression.
%
% Xrls is the X values matrix to be taken into account for RLS.
% yrls is the y values vector to be taken into account for RLS.
% s is RLS scale parameter.
% y is the vector of the dependent variable.
% X is the data matrix of the independent variable.
%
% Reference:
% Rousseeuw PJ, Leroy AM (1987): Robust regression and outlier detection. Wiley.
%
%
% Alexandros Leontitsis
% Institute of Mathematics and Statistics
% University of Kent at Canterbury
% Canterbury
% Kent, CT2 7NF
% U.K.
%
% University e-mail: al10@ukc.ac.uk (until December 2002)
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% Sep 3, 2001.

if nargin<1 | isempty(y)==1
   error('Not enough input arguments.');
end

% Estimate the LMS values
if nargin<2 | isempty(X)==1
   LMSout=LMSreg(y);
   X=(1:length(y))';
else
   LMSout=LMSreg(y,X);
end

% p is the number of parameters to be estimated
p=size(X,2)+1;

% Calculate the residuals
r=y-LMSout;

% Estimate the preliminary scale parameter
s=LMSsca(r,0,p);

% Take into account a data point, if its residual is relatively small
w=find(abs(r/s)<=2.5);
Xrls=X(w,:);
yrls=y(w);


%computes the correlation (matrix) sequence given a stable ss description
function [corrmat]=ss2corr_v000(A, Q, C, P0, dt, crind)

nout=size(C,1);
nout=nout*(nout+1)/2;
nst=size(A,1);
corrmat=zeros(nout,length(crind));

%compute the initial covariance
if (isempty(P0))
    %check whether system is stable or not
    eigenval=eig(A);
    if (~isempty(find(eigenval>=1,1)))
        disp('A is not stable');
        eigenval
        return;
    else
        %Use lyapunov eq.
        P0=dlyap(A,Q);
    end
end

%convert the discrete time into cont
crcind=crind*dt;
[Ac, Qc]=dc2dc_v000(A, Q, [], dt, 1,[]);

%compute the covariances
for in=1:length(crind)
    sr_a=crcind(in);
    Ad=expm(Ac*abs(sr_a));
    %[Ad, Qd]=dc2dc_v000(Ac, Qc, [], abs(crcind(in)), 0,[]);
    if (sr_a>0)
        mx_a=C*(Ad*P0)*C';
    elseif (sr_a<0)
        mx_a=C*(P0*Ad')*C';
    else
        mx_a=C*P0*C';
    end
    corrmat(:,in)=mat2vec(mx_a);
end

function vec=mat2vec(mat)
nrow=size(mat,1);
nout=nrow*(nrow+1)/2;
vec=zeros(nout,1);
lb=1;
ub=nrow;
for in=1:size(mat,1)
    vec(lb:ub)=diag(mat,in-1);
    lb=ub+1;
    ub=ub+nrow-in;
end


%  generate the Companion matrix of the polynomials f
%
%  Syntax:   >> C = CompanionMatrix(f)
%
%  Input:      f --- (string or coefficient vector) polynomial
%
%  Output:     C --- (matrix) The companion matrix
%
%  Example:  >> CompanionMatrix('x^4 + 2*x^3 + 3*x^2 + 4*x+5')
%
%            ans =
%
%                -2    -3    -4    -5
%                 1     0     0     0
%                 0     1     0     0
%                 0     0     1     0


function res = fact1(x)
if x > 0
   res = x * fact1(x-1);
else
   res = 1;
end


% Compute the error of a pose-pose constraint
% x1 3x1 vector (x,y,theta) of the first robot pose
% x2 3x1 vector (x,y,theta) of the second robot pose
% z 3x1 vector (x,y,theta) of the measurement
%
% You may use the functions v2t() and t2v() to compute
% a Homogeneous matrix out of a (x, y, theta) vector
% for computing the error.
%
% Output
% e 3x1 error of the constraint
% A 3x3 Jacobian wrt x1
% B 3x3 Jacobian wrt x2
function [e, A, B] = linearize_pose_pose_constraint(x1, x2, z)

  % TODO compute the error and the Jacobians of the error
    X1 = v2t(x1);
    X2 = v2t(x2);
    Z = v2t(z);
    e = t2v(Z\(X1\X2));

    Rij = Z(1:3,1:3);
    Ri = X1(1:3,1:3);

    thi = atan2(Ri(2,1),Ri(1,1));
    thij = atan2(Rij(2,1),Rij(1,1));

    xi = x1(1);
    yi = x1(2);
    xj = x2(1);
    yj = x2(2);

%    A = [-Rij'*Ri' 0 0; 0 -Rij'*Ri' 0; 0 0 -1];
%    B = [Rij'*Ri' 0 0; 0 Rij'*Ri' 0; 0 0 1];

    A = [-cos(thi)*cos(thij)+sin(thi)*sin(thij) -sin(thi)*cos(thij)-cos(thi)*sin(thij) 0; cos(thi)*sin(thij)+sin(thi)*cos(thij) sin(thi)*sin(thij)-cos(thi)*cos(thij) 0; 0 0 -1];

    A(1:2,3) = [cos(thij)*(-sin(thi)*(xj-xi)+cos(thi)*(yj-yi))+sin(thij)*(-cos(thi)*(xj-xi)-sin(thi)*(yj-yi)); -sin(thij)*(-sin(thi)*(xj-xi)+cos(thi)*(yj-yi))+cos(thij)*(-cos(thi)*(xj-xi)-sin(thi)*(yj-yi))];

    B = [cos(thi)*cos(thij)-sin(thi)*sin(thij) sin(thi)*cos(thij)+cos(thi)*sin(thij) 0; -cos(thi)*sin(thij)-sin(thi)*cos(thij) -sin(thi)*sin(thij)+cos(thi)*cos(thij) 0; 0 0 1];

end;


%
% Example #1  Best-Fit Circle
%
%{

% pseudo-data setting
t=-pi:.01:pi;
x=sin(t);
y=cos(t);
xn=.05*randn(1,629);
yn=.05*randn(1,629);
x=x+xn;
y=y+yn;

% additional data for objective function
mydata.x=x;
mydata.y=y;

ds(1,'circlefit',mydata,10,3,-10,10,2000)

plot(x,y,'o','markersize',2);
hold on
plotcircle(globalminimizer(1),globalminimizer(2),globalminimizer(3),'r')
daspect([1 1 1])
shg

%}
function out=circlefit(X,mydata)

xdata=mydata.x;
ydata=mydata.y;

[N,~]=size(X);
out=ones(N,1); % pre-memory
for i=1:N
    x=X(i,:);
    a=x(1);
    b=x(2);
    r=x(3);

    out(i)=sum(abs((xdata-a).^2+(ydata-b).^2-repmat(r.^2,[size(xdata,1),1])));
end

% out---> Nx1 sized,  where N:size of superorganism (i.e., population size; pattern-matrix size)


function [Rob,Sen,Lmk,Obs,Frm,Fac] = solveGraphQR(Rob,Sen,Lmk,Obs,Frm,Fac,options)

% SOLVEGRAPHQR Solves the SLAM graph using QR decomposition.
%   [Rob,Sen,Lmk,Obs,Frm,Fac] = SOLVEGRAPHQR(Rob,Sen,Lmk,Obs,Frm,Fac)
%   solves the graph-SLAM problem using QR decomposition of the
%   Hessian matrix.
%
%   IMPORTANT NOTE: This method is illustrative and constitutes the
%   motivation for this toolbox. One can achieve better performances, both
%   in computing time and possibly in robustness and accuracy, by using
%   Matlab's built-in nonlinear optimization tools, such as LSQNONLIN.
%
%   See courseSLAM.pdf in the documentation for details about the QR
%   decomposition for solving the graph-SLAM problem.
%
%   See also SOLVEGRAPHCHOLESKY, ERRORSTATEJACOBIANS, UPDATESTATES,
%   COMPUTEERROR, COMPUTERESIDUAL, COLAMD, '\', MLDIVIDE, LSQNONLIN.

% Copyright 2015-    Joan Sola @ IRI-UPC-CSIC.

global Map

% Control of iterations and exit conditions
n_iter      = options.niterations; % exit criterion of number of iterations
target_dres = options.target_dres; % exit criterion for error variation
target_res  = options.target_res;  % exit criterion for current residual
res_old     = 1e10;                   % last iteration's error

% Map states range
Map.mr = find(Map.used);
% Map factors range
errs = [Fac.err];
Map.fr = 1:sum([errs.size]);

for it = 1:n_iter

    % Compute Jacobians for projection onto the manifold
    [Frm,Lmk] = errorStateJacobians(Frm,Lmk);

    % Build Hessian A and rhs vector b, in global Map
    Fac       = buildProblem(Rob,Sen,Lmk,Obs,Frm,Fac);

    if it == 1 % do this only once:

        % Column permutation
        p  = colamd(Map.A(Map.fr,Map.mr))';

        % Permutated map range
        pr = Map.mr(p);
        Map.pr = pr;

    end

    % Decomposition
    [Map.d, Map.R] = qr(Map.A(Map.fr,pr), Map.b(Map.fr), 0);

    % Solve for dx and reorder:
    %   - dx is Map.x(mr)
    %   - reordered dx is Map.x(pr)
    Map.x(pr) = -Map.R\Map.d; % solve for dx;

    % NOTE: Matlab is able to do all the reordering and QR factorization
    % for you. If you just use the operator '\', as in 'dx = -A\b', Matlab
    % will reorder A, then factor it to get R and d, then solve the
    % factored problem, then reorder back the result into dx. Use the
    % following line to accomplish this, and comment out the code from line
    % 'if it == 1' until here:
    %
    %     Map.x(Map.mr) = -Map.A(Map.fr,Map.mr)\Map.b(Map.fr);

    % Update nominal states
    [Rob,Lmk,Frm] = updateStates(Rob,Lmk,Frm);

    % Check resulting errors
    [res, err_max] = computeResidual(Rob,Sen,Lmk,Obs,Frm,Fac);
    dres           = res - res_old;
    res_old        = res;

    % Test and exit
    if ( ( -dres <= target_dres ) || (err_max <= target_res) ) %&& ( abs(derr) < target_derr) )
        break;
    end

end

% Compute full covariance matrix.
Map.P(pr,pr) = inv(full(Map.R))*inv(full(Map.R))';

end

function Fac = buildProblem(Rob,Sen,Lmk,Obs,Frm,Fac)

% BUILDPROBLEM Build least squares problem's matrix A and vector b
%   Fac = BUILDPROBLEM(Rob,Sen,Lmk,Obs,Frm,Fac) Builds the least squares
%   problem's matrix A and vector b for a solution using sparse QR
%   factorization of A.

global Map

% Reset Hessian and rhs vector
Map.A(Map.fr,Map.mr) = 0;
Map.b(Map.fr)    = 0;

% Iterate all factors
facCount = 1;
for fac = find([Fac.used])

    % Extract some pointers
    rob    = Fac(fac).rob;
    sen    = Fac(fac).sen;
    lmk    = Fac(fac).lmk;
    frames = Fac(fac).frames;

    % Compute factor error, info mat, and Jacobians
    [Fac(fac), e, ~, Wsqrt, J1, J2, r1, r2] = computeError(...
        Rob(rob),       ...
        Sen(sen),       ...
        Lmk(lmk),       ...
        Obs(sen,lmk),   ...
        Frm(frames),    ...
        Fac(fac));

    % row band matrix size
    m  = numel(e);
    mr = (facCount : facCount + m - 1);

    % Update A and b
    Map.A(mr,r1) = Wsqrt * J1;
    Map.A(mr,r2) = Wsqrt * J2;

    Map.b(mr,1)  = Wsqrt * e;

    % Advance to next row band
    facCount = facCount + m;

end

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function phi=sinCosConformLat2EllipsLat(sinChi,cosChi,f)
%%SINCOSCONFORMLAT2ELLIPSLAT Given the sine and cosine of a conformal
%           latitude, determine an ellipsoidal (geodetic) latitude. A
%           conformal latitude is a latitude in terms of a sphere that is
%           conformal with regard to the ellipsoid. That is, the
%           transformation from the ellipsoid to the sphere preserves
%           angles. This is such that the angle of intersection  between
%           any two lines on the ellipsoid is the same as the corresponding
%           angle on the sphere.
%
%INPUTS: sinChi The real sine of the conformal latitude. This can be a
%               scalar or a matrix of values to convert.
%        cosChi The real cosine of the conformal latitude. This can be a
%               scalar or a matrix of values to convert. This has to be the
%               same size as sinChi.
%             f The flattening factor of the reference ellipsoid. If this
%               argument is omitted, the value in Constants.WGS84Flattening
%               is used. This function is only accurate for values from 0
%               to about 0.999. Typically, this will be close to zero, as
%               in Constants.WGS84Flattening.
%
%OUTPUTS: phi The ellipsoidal latitudes in radians. This is the same size
%             as sinChi and cosChi.
%
%Conformal latitudes are discussed in Chapter 3 of [1]. This function
%implements the fixed-point iteration that is described in Section 2.9 of
%[2]. Smaller values of f lead to faster convergence. The maximum number of
%iterations in the function, which is typically never obtained, is set to
%be able to convert unusually large values of f up to about 0.999. A
%different fixed point iteration when given the conformal latitude itself
%is given in [3].
%
%REFERENCES:
%[1] J. P. Snyder, "Map projections- a working manual," U.S. Geological
%    Survey, Tech. Rep. 1395, 1987.
%[2] Office of Geomatics, "National geospatial-intelligence agency
%    standardization document: Implementation practice: The universal
%    grids and the transverse mercator and polar stereographic
%    map projections," 25 Mar. 2014. [Online]. Available:
%    http://earth-info.nga.mil/GandG/publications/NGA_SIG_0012_2_0_0_UTMUPS/NGA.SIG.0012_2.0.0_UTMUPS.pdf
%[3] Weisstein, Eric W. "Conformal Latitude." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/ConformalLatitude.html
%
%July 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(f))
    f=Constants.WGS84Flattening;
end

%The first numerical eccentricity of the ellipsoid.
e=sqrt(2*f-f^2);

N=numel(sinChi);
phi=zeros(size(sinChi));

for curPoint=1:N
    s=sinChi(curPoint);
    %It should converge in 7 or fewer iterations for
    %f=Constants.WGS84Flattening. The upper bound of 5e7 should be enough for
    %f=0.999.
    for curIter=1:3e7
        P=exp(e*atanh(e*s));
        term1=(1+sinChi(curPoint))*P*P;
        term2=(1-sinChi(curPoint));

        sNew=(term1-term2)/(term1+term2);

        if(abs(s-sNew)<=eps(s))
            break
        end
        s=sNew;
    end
    P=exp(e*atanh(e*s));
    c=(1/2)*((1+s)/P+(1-s)*P)*cosChi(curPoint);
    phi(curPoint)=atan2(s,c);
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 det = r8pbl_det ( n, mu, a_lu )

%*****************************************************************************80
%
%% R8PBL_DET computes the determinant of a matrix factored by R8PBL_FA.
%
%  Discussion:
%
%    The R8PBL storage format is for a symmetric positive definite band matrix.
%
%    To save storage, only the diagonal and lower triangle of A is stored,
%    in a compact diagonal format that preserves columns.
%
%    The diagonal is stored in row 1 of the array.
%    The first subdiagonal in row 2, columns 1 through MU.
%    The second subdiagonal in row 3, columns 1 through MU-1.
%    The MU-th subdiagonal in row MU+1, columns 1 through 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Bunch, Moler, Stewart,
%    LINPACK User's Guide,
%    SIAM, Philadelphia, 1979.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer MU, the upper (and lower) bandwidth.
%    MU must be nonnegative, and no greater than N-1.
%
%    Input, real A_LU(MU+1,N), the LU factors from R8PBL_FA.
%
%    Output, real DET, the determinant of the matrix.
%
  det = prod ( a_lu(1,1:n).^2 );

  return
end


function [ x, seed ] = hyperball01_sample ( m, n, seed )

%*****************************************************************************80
%
%% HYPERBALL01_SAMPLE uniformly samples the unit hyperball in M dimensions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Russell Cheng,
%    Random Variate Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998, pages 168.
%
%    Reuven Rubinstein,
%    Monte Carlo Optimization, Simulation, and Sensitivity
%    of Queueing Networks,
%    Krieger, 1992,
%    ISBN: 0894647644,
%    LC: QA298.R79.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(M,N), the points.
%
  x = randn ( m, n );
  norm = ones ( 1, m ) * ( x.^2 );
  norm = sqrt ( norm );
  for i = 1 : m
    x(i,1:n) = x(i,1:n) ./ norm(1:n);
  end

  for j = 1 : n
    r = rand ( 1, 1 );
    x(1:m,j) = r ^ ( 1.0 / m ) * x(1:m,j);
  end

  return
end


function b = NormalizeArray(a)
% Normalizes array a. This means that the minimum value will become 0 and
% the maximum value 1.
%
% a:            Input array.
%
% b:            Normalized output array
%
%     Jasper Uijlings - 2013

minVal = min(a(:));
maxVal = max(a(:));

diffVal = maxVal - minVal;

b = a - minVal;
if diffVal ~= 0
    b = b ./ diffVal;
end


function z = polyval2(p,x,y)
% POLYVAL2
% ---------
%
% Evaluate a 2D polynomial
% By SS Rogers (2006)
%
% Usage
% ------
% Z = POLYVAL2(P,X,Y) returns the value of a 2D polynomial P evaluated at (X,Y). P
% is a vector of length (N+1)*(N+2)/2 containing the polynomial coefficients in
% ascending powers:
%
%   P = [p00 p10 p01 p20 p11 p02 p30 p21 p12 p03...]
%
% e.g. For a 3rd order fit, polyval2.m evaluates the matrix equation:
%
%    Z = V*P    or
%
%                   2      2  3  2      2   3
%    Z = [1  x  y  x  xy  y  x  x y  x y   y ]  [p00
%                                                p10
%                                                p01
%                                                p20
%                                                p11
%                                                p02
%                                                p30
%                                                p21
%                                                p12
%                                                p03]
%
% *Note:* P is not in the format of standard Matlab 1D polynomials.
%
% X and Y should be vectors; the polynomial is evaluated at all
% points (X,Y).
%
% Class support for inputs P,X,Y:
%    float: double, single

x=x(:);
y=y(:);
lx=length(x);
ly=length(y);
lp=length(p);
pts=lx*ly;

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

n=(sqrt(1+8*length(p))-3)/2;
% Check input is a vector
if ~(isvector(p) || mod(n,1)==0 || lx==ly)
    error('MATLAB:polyval2:InvalidP',...
            'P must be a vector of length (N+1)*(N+2)/2, where N is order. X and Y must be same size.');
end

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

z=V*p';
z=reshape(z,ly,lx);


function [Mout, RHSout] = combineBC2D(BC, Meq, RHSeq)
%COMBINEBC This function combines the boundary condition equations with the
%main physical model equations, and delivers the matrix of coefficient and
%RHS to be solved for the internal cells.
%
% SYNOPSIS:
%   [Mout, RHSout] = combineBC2D(BC, Meq, RHSeq)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nxy = BC.domain.dims;
Nx = Nxy(1); Ny = Nxy(2);
G=reshape(1:(Nx+2)*(Ny+2), Nx+2, Ny+2);
dx = BC.domain.cellsize.x;
dy = BC.domain.cellsize.y;

% define the RHS column vector
ms = size(Meq);
M = Meq;
RHS = RHSeq;

% Assign values to the boundary condition matrix and the RHS vector based
% on the BC structure
% top boundary
j=Ny+2;
i=2:Nx+1;
top = reshape(sub2ind(ms, G(i,j-1), G(i,j-1)), Nx,1); % top boundary cells
topN = reshape(sub2ind(ms, G(i,j-1), G(i,j)), Nx, 1); % north cells to top boundary cells
M(top) = M(top)-((BC.top.b/2 - BC.top.a/dy(end))./(BC.top.b/2 + BC.top.a/dy(end))).*M(topN);
RHS(G(i,j-1)) = RHS(G(i,j-1))-M(topN).*BC.top.c./(BC.top.b/2 + BC.top.a/dy(end));

% Bottom boundary
j=1;
i=2:Nx+1;
bottom = reshape(sub2ind(ms, G(i,j+1), G(i,j+1)), Nx,1); % bottom boundary cells
bottomS = reshape(sub2ind(ms, G(i,j+1), G(i,j)), Nx, 1); % south cells to bottom boundary cells
M(bottom) = M(bottom)-((BC.bottom.b/2 + BC.bottom.a/dy(1))./(BC.bottom.b/2 - BC.bottom.a/dy(1))).*M(bottomS);
RHS(G(i,j+1)) = RHS(G(i,j+1))-M(bottomS).*BC.bottom.c./(BC.bottom.b/2 - BC.bottom.a/dy(1));

% Right boundary
i=Nx+2;
j=2:Ny+1;
right = reshape(sub2ind(ms, G(i-1,j), G(i-1,j)), Ny,1); % right boundary cells
rightE = reshape(sub2ind(ms, G(i-1,j), G(i,j)), Ny, 1); % east cells to right boundary cells
M(right) = M(right)-((BC.right.b/2 - BC.right.a/dx(end))./(BC.right.b/2 + BC.right.a/dx(end)))'.*M(rightE);
RHS(G(i-1,j)) = RHS(G(i-1,j))-M(rightE).*(BC.right.c./(BC.right.b/2 + BC.right.a/dx(end)))';

% Left boundary
i = 1;
j=2:Ny+1;
left = reshape(sub2ind(ms, G(i+1,j), G(i+1,j)), Ny,1); % left boundary cells
leftW = reshape(sub2ind(ms, G(i+1,j), G(i,j)), Ny, 1); % west cells to left boundary cells
M(left) = M(left)-((BC.left.b/2 + BC.left.a/dx(1))./(BC.left.b/2 - BC.left.a/dx(1)))'.*M(leftW);
RHS(G(i+1,j)) = RHS(G(i+1,j))-M(leftW).*(BC.left.c./(BC.left.b/2 - BC.left.a/dx(1)))';

Mout = M(G(2:end-1,2:end-1), G(2:end-1,2:end-1));
RHSout = RHS(reshape(G(2:end-1,2:end-1),Nx*Ny,1));


function Distance = CalDistance(PopObj,RefPoint)
% Calculate the distance between each solution to each adjusted reference
% point

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

    N  = size(PopObj,1);
    NR = size(RefPoint,1);

    %% Adjust the location of each reference point
    index=find(sum(PopObj.^2,2)==0);
    if ~isempty(index)
        PopObj(index,:)=PopObj(index,:)+1e-06*rand(size(PopObj(index,:)));
    end
    Cosine = 1 - pdist2(PopObj,RefPoint,'cosine');%The cosine of the individual and the reference point in the population, N*NR
    NormR  = sqrt(sum(RefPoint.^2,2)); %NR*1
    NormP  = sqrt(sum(PopObj.^2,2)); %N*1
    d1     = repmat(NormP,1,NR).*Cosine; %N*NR
    d2     = repmat(NormP,1,NR).*sqrt(1-Cosine.^2); %N*NR
    [~,nearest] = min(d2,[],1); %1*NR
    RefPoint    = RefPoint.*repmat(d1(N.*(0:NR-1)+nearest)'./NormR,1,size(RefPoint,2));

    %% Calculate the distance between each solution to each point
    Distance = pdist2(PopObj,RefPoint);
end


function [ x, y, z, w ] = ld0194 ( )

%*****************************************************************************80
%
%% LD0194 computes the 194 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(194,1);
  y = zeros(194,1);
  z = zeros(194,1);
  w = zeros(194,1);
  a = 0.0;
  b = 0.0;
  v = 0.1782340447244611E-02;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.5716905949977102E-02;
  [ n, x, y, z, w ] = gen_oh ( 2, n, a, b, v, x, y, z, w );
  v = 0.5573383178848738E-02;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );
  a = 0.6712973442695226;
  v = 0.5608704082587997E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.2892465627575439;
  v = 0.5158237711805383E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.4446933178717437;
  v = 0.5518771467273614E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.1299335447650067;
  v = 0.4106777028169394E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.3457702197611283;
  v = 0.5051846064614808E-02;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.1590417105383530;
  b = 0.8360360154824589;
  v = 0.5530248916233094E-02;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );

  return
end


function const_pts = rectQAM_const(M)
%

% M-PAM bit/symbol ordering
[bo so] = PAM_Gray_Code;

k = log2(M);
I = 2^ceil(k/2);
Q = 2^floor(k/2);

% Produce PAM constellation points for in-phase and quadrature
I_t = -(I-1):2:I-1;
Q_t = Q-1:-2:-(Q-1);

% Re-order based on the PAM Gray mapping
for i=1:length(I_t)
    I_pts(so{log2(I)}(i)+1) = I_t(i);
end
for i=1:length(Q_t)
    Q_pts(so{log2(Q)}(i)+1) = Q_t(i);
end

for i=0:M-1
    MSBs = floor(i/I);
    LSBs = i-MSBs*I;
    QAM_I(i+1) = I_pts(LSBs+1);
    QAM_Q(i+1) = Q_pts(MSBs+1);
end

const_pts=complex(QAM_I,QAM_Q);


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

%%Function description:
%%Transformation of the end effector frame following the Euler angles
%%in closed form
%%input: rotation angles along y,z,y axis expressed in degrees

function [EulerMat]=Euler(fi1,theta,fi2)

EulerMat=[cosd(fi1)*cosd(theta)*cosd(fi2)-sind(fi1)*sind(fi2),-cosd(fi1)*cosd(theta)*sind(fi2)-sind(fi1)*cosd(fi2),cosd(fi1)*sind(theta),0;
        sind(fi1)*cosd(theta)*cosd(fi2)+cosd(fi1)*sind(fi2),-sind(fi1)*cosd(theta)*sind(fi2)+cosd(fi1)*cosd(fi2),sind(fi1)*sind(theta),0;
        -sind(theta)*cosd(fi2),sind(theta)*sind(fi2),cosd(theta),0;
        0                      ,0                   ,0          ,1;];

end


% Test file for fractional calculus.

function pass = test_fracInt(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

tol = 100*pref.chebfuneps;
dom = [0, 1];

% Test values:
xx = linspace(dom(1)+.1, dom(2)-.1, 10)';

%% Polynomials
x = chebfun('x', dom);
q = sqrt(2)/2;
k = 1;
for n = [1, 4]

    U = diff(x.^n, q);
    tru = gamma(n+1)./gamma(n+1-q)*chebfun(@(x) x.^(n-q), dom, 'exps', [n-q, 0]);

    err(k) = norm(tru(xx) - U(xx), inf);
    tol(k) = 10*eps*vscale(U)*hscale(U);
    k = k + 1;

end

%% Exponential
u = chebfun('exp(x)', dom);
trueC = chebfun('erf(sqrt(x)).*exp(x) + 1./sqrt(pi*x)', dom, 'exps', [-.5 0]);
trueRL = chebfun('erf(sqrt(x)).*exp(x)', dom, 'exps', [.5, 0]);

% RL
U = diff(u, .5, 'Caputo');
err(3) = norm(trueRL(xx) - U(xx), inf);
tol(3) = 10*eps*vscale(U)*hscale(U);

% Caputo
U = diff(u, .5, 'RL');
err(4) = norm(trueC(xx) - U(xx), inf);
tol(4) = 1e2*eps*vscale(U)*hscale(U);

%% Integrate twice:
xx = linspace(-sqrt(2)*pi+.1, pi-.1, 10).';
f = chebfun(@sin, [-sqrt(2)*pi, pi]);
F = cumsum(f);
G = fracInt(fracInt(f, .3), .7);
err(5) = norm(feval(F, xx) - feval(G, xx), inf);
tol(5) = 10*eps*vscale(G)*hscale(G);

%% Differentiate twice:
xx = linspace(-sqrt(2)*pi+.1, pi-.1, 10)';
f = chebfun(@sin, [-sqrt(2)*pi, pi]);
F = diff(f);
G = fracDiff(fracDiff(f, .3), .7);
err(6) = norm(feval(F, xx) - feval(G, xx), inf);
tol(6) = 1e2*eps*vscale(G)*hscale(G);

%% Test quasimatrix support
x = chebfun('x', [0 1]);
V = cheb2quasi(vander(x, 5));
F = diff(V, .5);
err(7) = 0;
for k = 1:numel(V)
    err(7) = err(7) + norm(diff(V(:,k), .5) - F(:,k));
end
tol(7) = 10*eps;

%%
pass = err < tol;

end


function [xECEF,MInv]=ENU2ECEF(plhOrigin,xENU,a,f)
%%ENU2ECEF Convert from a local East-North-Up (ENU) Cartesian cooridnate
%          system to an Earth-centered Earth-fixed (ECEF) Cartesian
%          coordinate system. The alignment of the coordinate axes with the
%          reference ellipsoid is the one used in common standards such as
%          that used by the DoD's WGS-84 standard and the International
%          Earth Rotation and Reference Systems Service's (IERS)
%          international terrestrial reference frame (ITRF). The global
%          ECEF z-axis is North.
%
%INPUTS: plhOrigin A 3X1 point in [latitude;longitude; ellipsoidal height]
%             coordinates with respect to a reference ellipsoid
%             parameterized by a and f, derving as the origin of the local
%             ENU coordinate system. Latitude and longitude are given in
%             radians North and East.
%        xENU A 3XnumPts collection of 3D Cartesian points in the local ENU
%             coordinate system that should be converted to a global ECEF
%             coordinate system.
%           a The semi-major axis of the reference ellipsoid. If this
%             argument is omitted, the value in
%             Constants.WGS84SemiMajorAxis is used.
%           f The flattening factor of the reference ellipsoid. If this
%             argument is omitted, the value in Constants.WGS84Flattening
%             is used.
%
%OUTPUTS: xECEF A 3XnumPts matrix of the points in xENU converted to ECEF
%               coordinates.
%          MInv A rotation matrix such that rotates a point from ENU to
%               ECEF coordinates.
%
%The ENU coordinate system is a common local tangent plane cooridnate
%system. Here, we take "up" to be that described by a reference ellipsoid
%and not true gravitational "up". This limits the accuracy of the local
%coordinate system.
%
%The conversions are mentioned in [1].
%
%EXAMPLE:
%Here, we convert 3 points in a local ENU tangent-plane coordinate system
%to ECEF and then to ellipsoidal coordinates. In the ENU coordinate system,
%the points are offset in the North, East, and up directions. One will see
%that in ellipsoidal coordinates, as compared to the origin, there are
%respective offsets in latitude (with no change in longitude), longitude
%(with no change in latitude) and heigh (with no change in latitude or
%longitude).
% lat=19.7241*(pi/180);
% lon=-155.0868*(pi/180);
% height=0;
% plhPoint=[lat;lon;height];%Around Hilo, Hawaii.
% xENU=[0, 1e3,   0;
%     1e3,   0,   0;
%       0,   0, 1e3];
% xECEF=ENU2ECEF(plhPoint,xENU);
% latLonHeight=Cart2Ellipse(xECEF);
% diffVals=bsxfun(@minus,latLonHeight,plhPoint);
% %Convert angles to degrees
% diffVals(1:2,:)=diffVals(1:2,:)*(180/pi);
% %One can see the change in latitude on the first point, the change in
% %longitude on the second point, and no change in laittude or longitude in
% %the third point.
% diffVals(1:2,:)
% %The change in height with the third point is the full kilometer. The
% %changes with the other points is just due to the tangent point
% %approximation and are much smaller.
% diffVals(3,:)
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%
%September 2020 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<4||isempty(f))
    f=Constants.WGS84Flattening;
end

MInv=getENUAxes(plhOrigin,false,a,f);
originCart=ellips2Cart(plhOrigin,a,f);

numPts=size(xENU,2);
xECEF=zeros(3,numPts);
for k=1:numPts
    xECEF(:,k)=xENU(1,k)*MInv(:,1)+xENU(2,k)*MInv(:,2)+xENU(3,k)*MInv(:,3);
end

%Correct for the origin shift.
xECEF=bsxfun(@plus,xECEF,originCart);
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 [invA]=invltod(A,n)

% computes the inverse of a n*n lower triangular matrix with ones on the main diagonal
% uses result (XXX) from Appendix (XXX)

% first obtain the B matrix from A
B=sparse(tril(A,-1));

% compute the summation term
summ=sparse(n,n);
prodt=speye(n);
for ii=1:n-1
prodt=prodt*B;
summ=summ+(-1)^ii*prodt;
end

% compute the inverse of A
invA=full(speye(n)+summ);


function buckling_spring_theta ( l_num, theta_num )

%*****************************************************************************80
%
%% BUCKLING_SPRING_THETA plots LAMBDA(L,THETA) and MU(L,THETA) lines for fixed L.
%
%  Discussion:
%
%    This routine fixes L and plots LAMBDA and MU as functions of THETA.
%
%  Usage:
%
%    buckling_spring_theta ( l_num, theta_num )
%
%    * l_num is the number of L values along each THETA line;
%    * theta_num is the number of THETA lines to draw.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer L_NUM, the number of values of L,
%    which controls the smoothness of each line.
%    A value of 101 is plenty.
%
%    Input, integer THETA_NUM, the number of values of THETA,
%    which sets the number of lines to be drawn.
%    A value of 101 is plenty.
%
  l_lo = 0.25;
  l_hi = 1.75;

  theta_lo = - 3 * pi / 8;
  theta_hi = + 3 * pi / 8;

  for j = 1 : l_num

    if ( l_num == 1 )
      L = 0.5 * ( l_lo + l_hi );
    else
      L = ( ( l_num - j     ) * l_lo ...
          + (         j - 1 ) * l_hi ) ...
          / ( l_num     - 1 );
    end

    for i = 1 : theta_num

      if ( theta_num == 1 )
        theta = 0.5 * ( theta_lo + theta_hi );
      else
        theta = ( ( theta_num - i     ) * theta_lo ...
                + (             i - 1 ) * theta_hi ) ...
                / ( theta_num     - 1 );
      end

      lambda(i,j) = ( 1 - L ) * cos ( theta ) + theta * sin ( theta ) / 4 / L;
      mu(i,j) = - theta * cos ( theta ) / 2 / L + 2 * ( 1 - L ) * sin ( theta );

    end

  end

  plot ( lambda, mu )
%
%  Limit the plot range.
%  Suppress this command to see the whole plot.
%
  axis ( [ 0.1 0.9 -0.07 0.07 ] );
%
%  Label the plot.
%
  xlabel ( '\lambda(L,\theta)' );
  ylabel ( '\mu(L,\theta)' );
  title ( 'Each line is a fixed value of L' );

  return
end


% DOC
%
% Files
%   afemdoc              - Adaptive Finite Element Method
%   amgdoc               - ALGEBRAIC MULTIGRID METHOD
%   amgdoctest1          - AMG TEST I: DIFFERENT MESHES
%   amgdoctest2          - AMG TEST II: Different boundary conditions
%   amgdoctest3          - AMG TEST III: Robustness to time discretization
%   amgtest              - test algebraic mutligrid solver
%   amgtest2             - AMG TEST II: Different boundary conditions
%   auxstructuredoc      - Auxiliary Mesh Data Structure
%   bddoc                - Data Structure: Boundary Conditions
%   bisectdoc            - Bisection in Two Dimensions
%   coarsenAMGdoc        - Coarsening for Algebraic Multigrid
%   coarsendoc           - COARSENING in TWO DIMENSIONS
%   coloringdoc          - Coloring Vertices of a Graph
%   dof3edgedoc          - Data Structure: Lowest Order Edge Element
%   dofdoc               - Data Sturcture for Degree of Freedom (DOF)
%   dofedgedoc           - Dofedge in Two Dimensions
%   dofP2doc             - Data Structure: P2 Quadratic Element
%   fastcoding           - Vectorization
%   femcontent           - Finite Element Methods
%   femdoc               - Finite Element Methods
%   ifem                 - Display HTML documentation in the MATLAB web browser.
%   introduction         - Introduction of iFEM
%   Maxwell1doc          - Equation: Maxwell Equation Discretized by Edge Element
%   Maxwell1testdoc      - Edge Element Discretization of Maxwell Equations: Linear Element
%   Maxwell2doc          - Equation: Maxwell equation Quadratic Element in 3D
%   Maxwell2testdoc      - Edge Element Discretization of Maxwell Equations
%   Maxwelldoc           - Equation: Maxwell Equation Discretized by Edge Element
%   MaxwellNeumanBCdoc   - Local Labeling of DOFs
%   Maxwelltestdoc       - Edge Element Discretization of Maxwell Equations
%   meshamrdoc           - Adaptive Mesh Refinement/Coarsening
%   meshbasicdoc         - Basic Data Structure representing a Mesh
%   meshdoc              - Mesh
%   meshoptdoc           - Mesh Smoothing and Optimization
%   mgdoc                - MULTIGRID on BISECTION GRIDS
%   mixFEMsolverdoc      - Mixed FEM solver
%   Poisson3BDM1doc      - Equation: Poisson Equation Discretized by $BDM_1$ Element in 3D
%   Poisson3RT0doc       - Equation: Poisson Equation Discretized by $RT_0$ Element in 3D
%   PoissonBDM1doc       - Equation: Poisson Equation Discretized by $BDM_1$ Element in 2D
%   PoissonRT0doc        - Equation: Poisson Equation Discretized by $RT_0$ Element in 2D
%   sc3doc               - Simplicial Complex in Three Dimensions
%   scdoc                - Simplicial Complex in Three Dimensions
%   simplicialcomplexdoc - Simplicial Complex in 3D
%   solverdoc            - Multigrid Method
%   sparsematrixdoc      - Vectorization using Sparse Matrices
%   uniformrefine3doc    - 3-D Red Refinement


function [h_1d,h_2d] = create_h(sigma_h, size_h, opt, mode)

if sigma_h > opt.M || sigma_h > opt.N
    error('blur kernel must be smaller than image');
end

if mode == 1
    x = linspace(-size_h/2, size_h/2, size_h);
    h_1d = exp(-x.^2 / (2 * sigma_h^2)); % 1D Guassian filter kernel
    h_1d = h_1d / sum(h_1d); % normalize
    h_2d = kron(h_1d',h_1d);
elseif mode == 2
    h_2d = ones(size_h,size_h) / (size_h*size_h);
end


% Generate Figure 6 of the "Deep Scattering Spectrum" paper.

% Load the signal.
x = wavread('chord_signal.wav');

% Prepare the filters and scattering operators.
filt_opt.filter_type = {'gabor_1d','morlet_1d'};
filt_opt.B = [4 4];
filt_opt.Q = 4*filt_opt.B;
filt_opt.J = T_to_J([256 32768],filt_opt);

scat_opt.oversampling = 3;
scat_opt.M = 2;

Wop = wavelet_factory_1d(length(x), filt_opt, scat_opt);

% Compute scattering coefficients.
[S, U] = scat(x, Wop);

% Renormalize coefficients.
epsilon = 1e-3;
S = renorm_scat(S, epsilon);
S = renorm_1st(S, x, Wop{2}, epsilon);

S = resample_scat(S, 8, false);
U = resample_scat(U, 6, false);

% Compute the logarithm.
S = log_scat(S, 1e-3);
U = log_scat(U, 1e-3);

% Display scalogram U{2}, first-order coefficients S{2} and second-order
% coefficients S{3} for j1 = 58.
figure(6);
clf;
set(gcf,'Units','pixels','Position',[200 200 560 420]);
scattergram(U{2}, [], S{2}, [], S{3}, 58);
subplot(3,1,1);
set(gca,'XTick',[],'YTick',[]);
subplot(3,1,2);
set(gca,'XTick',[],'YTick',[]);
subplot(3,1,3);
set(gca,'XTick',[],'YTick',[]);
colormap(jet);


% test the spherical compression scheme

sampling_type = 'random';
% sampling_type = 'uniform';

data_type = 'syntetic';
% data_type = 'real';

n = 1500;
k = 2;          % number of vanishing moments.
dir = 1;

switch sampling_type
    case 'random'
        pos = randn(3,n);
        d = sqrt( pos(1,:).^2+pos(2,:).^2+pos(3,:).^2 );
        pos(1,:) = pos(1,:)./d;
        pos(2,:) = pos(2,:)./d;
        pos(3,:) = pos(3,:)./d;
    case 'uniform'
        [pos,L,diam] =  sphere_sampling(n);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute some function
switch data_type
    case 'syntetic'
        v = pos(1,:).^2 - pos(2,:).*pos(3,:);
        v = v(:);
        v = rescale(v);
        v = cos(10*v);
        % list for compression plot
        m_list = [1,2,5,10,15];
    case 'real'
        sampling_type = 'real';
        load('data/data_lenglet.mat');
%        [pos,face] = gen_base_mesh('oct');
%        [pos,face] = subdivide_sphere(pos,face,2); pos = pos';
        v = ADC24_36_14(:);
        pos = grad_81';
        % symmetrize the data
        pos = [pos,-pos];
        v = [v;v];
        n = length(v);
        v = rescale(v);
        % list for compression plot
        m_list = [5,10,20,40,70];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute clustering
clear options;
options.ptmax = k^2;
options.part_type = 'kmeans';
[part,B,G] = dichotomic_grouping(pos,options);
% plot
clf;
subplot(1,2,1);
plot_spherical_partition(pos,part);
title('Clustering of sampling locations');

subplot(1,2,2);
plot_spherical_function(pos,v);
title('Function to transform (interpolated)');
saveas(gcf, ['function_',data_type, '_clustering_', sampling_type], 'png');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perform transform
clear options;
options.part_type = 'kmeans';
[w,info,part] = perform_alpert_transform_nd(v,pos,k, 1, options);
% try reconstruction just to check the transform is bijective ...
options.part = part;
v1 = perform_alpert_transform_nd(w,pos,k, -1,options);
norm(v-v1, 'fro') % should be 0

cv = rev_sort_abs(v);
cw = rev_sort_abs(w);
cv = l2error(v);
cw = l2error(w);

a = (1:n/2)';
x = log2(a);
y1 = cv(a);
y2 = cw(a);

% reglin
fiton = a;
A = a;
B = y2;
aa = log2(A(fiton));
bb = log2(B(fiton));
[coef,S] = polyfit(aa,bb,1);
disp( sprintf('----> 1D Alpert transform, decreasing=%.4f', coef(1)) );
reglin = log2(a)*coef(1)+coef(2);

clf;
hold on;
plot( x, log2(y1), x, log2(y2), x, reglin, 'b:' );
axis tight;
legend('Original error decreasing', 'Transformed error decreasing');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perform reconstruction
clf;
subplot(2,3,1);
plot_spherical_function(pos,v);
title('Original function');
s = 1;

for m = m_list
    s = s+1;
    w1 = keep_biggest(w, round(n*m/100) );
    % backward transform
    v1 = perform_alpert_transform_nd(w1,pos,k, -1,part);
    subplot(2,3,s);
    plot_spherical_function(pos,v1);
    err = round( 100*norm(v-v1, 'fro')^2/norm(v, 'fro')^2 );    % RMS error in %
    title(sprintf('%.1f %% of coef, err=%d%%', m, err  ));
end

saveas(gcf, ['compression_',data_type, '_clustering_', sampling_type], 'png');


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

%Fourier Transfrom and  inverse Fourier Transfrom  of various functions
% the order of the operations is a little different
%from the order that they appear on the book

%F{exp(-t^2)}
syms t w
x=exp(-t^2);
fourier(x)

int(x*exp(-j*w*t),t,-inf,inf)

%F^-1{1/(1+j*w)}
X=1/(1+j*w);
ifourier(X)

X=1/(1+j*w);
ifourier(X,t)

syms n
X=1/(1+j*w);
ifourier(X,n)

%F{exp(-t)*u(t)}
syms t w
x=exp(-t)*heaviside(t);
X=fourier(x,w)

%F{1}
x=1;
fourier(x,w)
syms s
fourier(x,s)
fourier(x)