text:
/* eigen/francis.c
 *
 * Copyright (C) 2006, 2007 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>

/*
 * This module computes the eigenvalues of a real upper hessenberg
 * matrix, using the classical double shift Francis QR algorithm.
 * It will also optionally compute the full Schur form and matrix of
 * Schur vectors.
 *
 * See Golub & Van Loan, "Matrix Computations" (3rd ed),
 * algorithm 7.5.2
 */

/* exceptional shift coefficients - these values are from LAPACK DLAHQR */
#define GSL_FRANCIS_COEFF1        (0.75)
#define GSL_FRANCIS_COEFF2        (-0.4375)

static inline void francis_schur_decomp(gsl_matrix * H,
                                        gsl_vector_complex * eval,
                                        gsl_eigen_francis_workspace * w);
static inline size_t francis_search_subdiag_small_elements(gsl_matrix * A);
static inline int francis_qrstep(gsl_matrix * H,
                                 gsl_eigen_francis_workspace * w);
static inline void francis_schur_standardize(gsl_matrix *A,
                                             gsl_complex *eval1,
                                             gsl_complex *eval2,
                                             gsl_eigen_francis_workspace *w);
static inline size_t francis_get_submatrix(gsl_matrix *A, gsl_matrix *B);
static void francis_standard_form(gsl_matrix *A, double *cs, double *sn);

/*
gsl_eigen_francis_alloc()

Allocate a workspace for solving the nonsymmetric eigenvalue problem.
The size of this workspace is O(1)

Inputs: none

Return: pointer to workspace
*/

gsl_eigen_francis_workspace *
gsl_eigen_francis_alloc(void)
{
  gsl_eigen_francis_workspace *w;

  w = (gsl_eigen_francis_workspace *)
      calloc (1, sizeof (gsl_eigen_francis_workspace));

  if (w == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for workspace", GSL_ENOMEM);
    }

  /* these are filled in later */

  w->size = 0;
  w->max_iterations = 0;
  w->n_iter = 0;
  w->n_evals = 0;

  w->compute_t = 0;
  w->Z = NULL;
  w->H = NULL;

  return (w);
} /* gsl_eigen_francis_alloc() */

/*
gsl_eigen_francis_free()
  Free francis workspace w
*/

void
gsl_eigen_francis_free (gsl_eigen_francis_workspace *w)
{
  RETURN_IF_NULL (w);
  free(w);
} /* gsl_eigen_francis_free() */

/*
gsl_eigen_francis_T()
  Called when we want to compute the Schur form T, or no longer
compute the Schur form T

Inputs: compute_t - 1 to compute T, 0 to not compute T
        w         - francis workspace
*/

void
gsl_eigen_francis_T (const int compute_t, gsl_eigen_francis_workspace *w)
{
  w->compute_t = compute_t;
}

/*
gsl_eigen_francis()

Solve the nonsymmetric eigenvalue problem

H x = \lambda x

for the eigenvalues \lambda using algorithm 7.5.2 of
Golub & Van Loan, "Matrix Computations" (3rd ed)

Inputs: H    - upper hessenberg matrix
        eval - where to store eigenvalues
        w    - workspace

Return: success or error - if error code is returned,
        then the QR procedure did not converge in the
        allowed number of iterations. In the event of non-
        convergence, the number of eigenvalues found will
        still be stored in the beginning of eval,

Notes: On output, the diagonal of H contains 1-by-1 or 2-by-2
       blocks containing the eigenvalues. If T is desired,
       H will contain the full Schur form on output.
*/

int
gsl_eigen_francis (gsl_matrix * H, gsl_vector_complex * eval,
                   gsl_eigen_francis_workspace * w)
{
  /* check matrix and vector sizes */

  if (H->size1 != H->size2)
    {
      GSL_ERROR ("matrix must be square to compute eigenvalues", GSL_ENOTSQR);
    }
  else if (eval->size != H->size1)
    {
      GSL_ERROR ("eigenvalue vector must match matrix size", GSL_EBADLEN);
    }
  else
    {
      const size_t N = H->size1;
      int j;

      /*
       * Set internal parameters which depend on matrix size.
       * The Francis solver can be called with any size matrix
       * since the workspace does not depend on N.
       * Furthermore, multishift solvers which call the Francis
       * solver may need to call it with different sized matrices
       */
      w->size = N;
      w->max_iterations = 30 * N;

      /*
       * save a pointer to original matrix since francis_schur_decomp
       * is recursive
       */
      w->H = H;

      w->n_iter = 0;
      w->n_evals = 0;

      /*
       * zero out the first two subdiagonals (below the main subdiagonal)
       * needed as scratch space by the QR sweep routine
       */
      for (j = 0; j < (int) N - 3; ++j)
        {
          gsl_matrix_set(H, (size_t) j + 2, (size_t) j, 0.0);
          gsl_matrix_set(H, (size_t) j + 3, (size_t) j, 0.0);
        }

      if (N > 2)
        gsl_matrix_set(H, N - 1, N - 3, 0.0);

      /*
       * compute Schur decomposition of H and store eigenvalues
       * into eval
       */
      francis_schur_decomp(H, eval, w);

      if (w->n_evals != N)
        return GSL_EMAXITER;

      return GSL_SUCCESS;
    }
} /* gsl_eigen_francis() */

/*
gsl_eigen_francis_Z()

Solve the nonsymmetric eigenvalue problem for a Hessenberg
matrix

H x = \lambda x

for the eigenvalues \lambda using the Francis double-shift
method.

Here we compute the real Schur form

T = Q^t H Q

with the diagonal blocks of T giving us the eigenvalues.
Q is the matrix of Schur vectors.

Originally, H was obtained from a general nonsymmetric matrix
A via a transformation

H = U^t A U

so that

T = (UQ)^t A (UQ) = Z^t A Z

Z is the matrix of Schur vectors computed by this algorithm

Inputs: H    - upper hessenberg matrix
        eval - where to store eigenvalues
        Z    - where to store Schur vectors
        w    - workspace

Notes: 1) If T is computed, it is stored in H on output. Otherwise,
          the diagonal of H will contain 1-by-1 and 2-by-2 blocks
          containing the eigenvalues.

       2) The matrix Z must be initialized to the Hessenberg
          similarity matrix U. Or if you want the eigenvalues
          of H, initialize Z to the identity matrix.
*/

int
gsl_eigen_francis_Z (gsl_matrix * H, gsl_vector_complex * eval,
                     gsl_matrix * Z, gsl_eigen_francis_workspace * w)
{
  int s;

  /* set internal Z pointer so we know to accumulate transformations */
  w->Z = Z;

  s = gsl_eigen_francis(H, eval, w);

  w->Z = NULL;

  return s;
} /* gsl_eigen_francis_Z() */

/********************************************
 *           INTERNAL ROUTINES              *
 ********************************************/

/*
francis_schur_decomp()
  Compute the Schur decomposition of the matrix H

Inputs: H     - hessenberg matrix
        eval  - where to store eigenvalues
        w     - workspace

Return: none
*/

static inline void
francis_schur_decomp(gsl_matrix * H, gsl_vector_complex * eval,
                     gsl_eigen_francis_workspace * w)
{
  gsl_matrix_view m;   /* active matrix we are working on */
  size_t N;            /* size of matrix */
  size_t q;            /* index of small subdiagonal element */
  gsl_complex lambda1, /* eigenvalues */
              lambda2;

  N = H->size1;
  m = gsl_matrix_submatrix(H, 0, 0, N, N);

  while ((N > 2) && ((w->n_iter)++ < w->max_iterations))
    {
      q = francis_search_subdiag_small_elements(&m.matrix);

      if (q == 0)
        {
          /*
           * no small subdiagonal element found - perform a QR
           * sweep on the active reduced hessenberg matrix
           */
          francis_qrstep(&m.matrix, w);
          continue;
        }

      /*
       * a small subdiagonal element was found - one or two eigenvalues
       * have converged or the matrix has split into two smaller matrices
       */

      if (q == (N - 1))
        {
          /*
           * the last subdiagonal element of the matrix is 0 -
           * m_{NN} is a real eigenvalue
           */
          GSL_SET_COMPLEX(&lambda1,
                          gsl_matrix_get(&m.matrix, q, q), 0.0);
          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          w->n_evals += 1;
          w->n_iter = 0;

          --N;
          m = gsl_matrix_submatrix(&m.matrix, 0, 0, N, N);
        }
      else if (q == (N - 2))
        {
          gsl_matrix_view v;

          /*
           * The bottom right 2-by-2 block of m is an eigenvalue
           * system
           */

          v = gsl_matrix_submatrix(&m.matrix, q, q, 2, 2);
          francis_schur_standardize(&v.matrix, &lambda1, &lambda2, w);

          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          gsl_vector_complex_set(eval, w->n_evals + 1, lambda2);
          w->n_evals += 2;
          w->n_iter = 0;

          N -= 2;
          m = gsl_matrix_submatrix(&m.matrix, 0, 0, N, N);
        }
      else if (q == 1)
        {
          /* the first matrix element is an eigenvalue */
          GSL_SET_COMPLEX(&lambda1,
                          gsl_matrix_get(&m.matrix, 0, 0), 0.0);
          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          w->n_evals += 1;
          w->n_iter = 0;

          --N;
          m = gsl_matrix_submatrix(&m.matrix, 1, 1, N, N);
        }
      else if (q == 2)
        {
          gsl_matrix_view v;

          /* the upper left 2-by-2 block is an eigenvalue system */

          v = gsl_matrix_submatrix(&m.matrix, 0, 0, 2, 2);
          francis_schur_standardize(&v.matrix, &lambda1, &lambda2, w);

          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          gsl_vector_complex_set(eval, w->n_evals + 1, lambda2);
          w->n_evals += 2;
          w->n_iter = 0;

          N -= 2;
          m = gsl_matrix_submatrix(&m.matrix, 2, 2, N, N);
        }
      else
        {
          gsl_matrix_view v;

          /*
           * There is a zero element on the subdiagonal somewhere
           * in the middle of the matrix - we can now operate
           * separately on the two submatrices split by this
           * element. q is the row index of the zero element.
           */

          /* operate on lower right (N - q)-by-(N - q) block first */
          v = gsl_matrix_submatrix(&m.matrix, q, q, N - q, N - q);
          francis_schur_decomp(&v.matrix, eval, w);

          /* operate on upper left q-by-q block */
          v = gsl_matrix_submatrix(&m.matrix, 0, 0, q, q);
          francis_schur_decomp(&v.matrix, eval, w);

          N = 0;
        }
    }

  /* handle special cases of N = 1 or 2 */

  if (N == 1)
    {
      GSL_SET_COMPLEX(&lambda1, gsl_matrix_get(&m.matrix, 0, 0), 0.0);
      gsl_vector_complex_set(eval, w->n_evals, lambda1);
      w->n_evals += 1;
      w->n_iter = 0;
    }
  else if (N == 2)
    {
      francis_schur_standardize(&m.matrix, &lambda1, &lambda2, w);
      gsl_vector_complex_set(eval, w->n_evals, lambda1);
      gsl_vector_complex_set(eval, w->n_evals + 1, lambda2);
      w->n_evals += 2;
      w->n_iter = 0;
    }
} /* francis_schur_decomp() */

/*
francis_qrstep()
  Perform a Francis QR step.

See Golub & Van Loan, "Matrix Computations" (3rd ed),
algorithm 7.5.1

Inputs: H - upper Hessenberg matrix
        w - workspace

Notes: The matrix H must be "reduced", ie: have no tiny subdiagonal
       elements. When computing the first householder reflection,
       we divide by H_{21} so it is necessary that this element
       is not zero. When a subdiagonal element becomes negligible,
       the calling function should call this routine with the
       submatrices split by that element, so that we don't divide
       by zeros.
*/

static inline int
francis_qrstep(gsl_matrix * H, gsl_eigen_francis_workspace * w)
{
  const size_t N = H->size1;
  size_t i;        /* looping */
  gsl_matrix_view m;
  double tau_i;    /* householder coefficient */
  double dat[3];   /* householder vector */
  double scale;    /* scale factor to avoid overflow */
  gsl_vector_view v2, v3;
  size_t q, r;
  size_t top = 0;  /* location of H in original matrix */
  double s,
         disc;
  double h_nn,     /* H(n,n) */
         h_nm1nm1, /* H(n-1,n-1) */
         h_cross,  /* H(n,n-1) * H(n-1,n) */
         h_tmp1,
         h_tmp2;

  v2 = gsl_vector_view_array(dat, 2);
  v3 = gsl_vector_view_array(dat, 3);

  if ((w->n_iter % 10) == 0)
    {
      /*
       * exceptional shifts: we have gone 10 iterations
       * without finding a new eigenvalue, try a new choice of shifts.
       * See LAPACK routine DLAHQR
       */
      s = fabs(gsl_matrix_get(H, N - 1, N - 2)) +
          fabs(gsl_matrix_get(H, N - 2, N - 3));
      h_nn = gsl_matrix_get(H, N - 1, N - 1) + GSL_FRANCIS_COEFF1 * s;
      h_nm1nm1 = h_nn;
      h_cross = GSL_FRANCIS_COEFF2 * s * s;
    }
  else
    {
      /*
       * normal shifts - compute Rayleigh quotient and use
       * Wilkinson shift if possible
       */

      h_nn = gsl_matrix_get(H, N - 1, N - 1);
      h_nm1nm1 = gsl_matrix_get(H, N - 2, N - 2);
      h_cross = gsl_matrix_get(H, N - 1, N - 2) *
                gsl_matrix_get(H, N - 2, N - 1);

      disc = 0.5 * (h_nm1nm1 - h_nn);
      disc = disc * disc + h_cross;
      if (disc > 0.0)
        {
          double ave;

          /* real roots - use Wilkinson's shift twice */
          disc = sqrt(disc);
          ave = 0.5 * (h_nm1nm1 + h_nn);
          if (fabs(h_nm1nm1) - fabs(h_nn) > 0.0)
            {
              h_nm1nm1 = h_nm1nm1 * h_nn - h_cross;
              h_nn = h_nm1nm1 / (disc * GSL_SIGN(ave) + ave);
            }
          else
            {
              h_nn = disc * GSL_SIGN(ave) + ave;
            }

          h_nm1nm1 = h_nn;
          h_cross = 0.0;
        }
    }

  h_tmp1 = h_nm1nm1 - gsl_matrix_get(H, 0, 0);
  h_tmp2 = h_nn - gsl_matrix_get(H, 0, 0);

  /*
   * These formulas are equivalent to those in Golub & Van Loan
   * for the normal shift case - the terms have been rearranged
   * to reduce possible roundoff error when subdiagonal elements
   * are small
   */

  dat[0] = (h_tmp1*h_tmp2 - h_cross) / gsl_matrix_get(H, 1, 0) +
           gsl_matrix_get(H, 0, 1);
  dat[1] = gsl_matrix_get(H, 1, 1) - gsl_matrix_get(H, 0, 0) - h_tmp1 -
           h_tmp2;
  dat[2] = gsl_matrix_get(H, 2, 1);

  scale = fabs(dat[0]) + fabs(dat[1]) + fabs(dat[2]);
  if (scale != 0.0)
    {
      /* scale to prevent overflow or underflow */
      dat[0] /= scale;
      dat[1] /= scale;
      dat[2] /= scale;
    }

  if (w->Z || w->compute_t)
    {
      /*
       * get absolute indices of this (sub)matrix relative to the
       * original Hessenberg matrix
       */
      top = francis_get_submatrix(w->H, H);
    }

  for (i = 0; i < N - 2; ++i)
    {
      tau_i = gsl_linalg_householder_transform(&v3.vector);

      if (tau_i != 0.0)
        {
          /* q = max(1, i - 1) */
          q = (1 > ((int)i - 1)) ? 0 : (i - 1);

          /* r = min(i + 3, N - 1) */
          r = ((i + 3) < (N - 1)) ? (i + 3) : (N - 1);

          if (w->compute_t)
            {
              /*
               * We are computing the Schur form T, so we
               * need to transform the whole matrix H
               *
               * H -> P_k^t H P_k
               *
               * where P_k is the current Householder matrix
               */

              /* apply left householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(w->H,
                                       top + i,
                                       top + q,
                                       3,
                                       w->size - top - q);
              gsl_linalg_householder_hm(tau_i, &v3.vector, &m.matrix);

              /* apply right householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(w->H,
                                       0,
                                       top + i,
                                       top + r + 1,
                                       3);
              gsl_linalg_householder_mh(tau_i, &v3.vector, &m.matrix);
            }
          else
            {
              /*
               * We are not computing the Schur form T, so we
               * only need to transform the active block
               */

              /* apply left householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(H, i, q, 3, N - q);
              gsl_linalg_householder_hm(tau_i, &v3.vector, &m.matrix);

              /* apply right householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(H, 0, i, r + 1, 3);
              gsl_linalg_householder_mh(tau_i, &v3.vector, &m.matrix);
            }

          if (w->Z)
            {
              /* accumulate the similarity transformation into Z */
              m = gsl_matrix_submatrix(w->Z, 0, top + i, w->size, 3);
              gsl_linalg_householder_mh(tau_i, &v3.vector, &m.matrix);
            }
        } /* if (tau_i != 0.0) */

      dat[0] = gsl_matrix_get(H, i + 1, i);
      dat[1] = gsl_matrix_get(H, i + 2, i);
      if (i < (N - 3))
        {
          dat[2] = gsl_matrix_get(H, i + 3, i);
        }

      scale = fabs(dat[0]) + fabs(dat[1]) + fabs(dat[2]);
      if (scale != 0.0)
        {
          /* scale to prevent overflow or underflow */
          dat[0] /= scale;
          dat[1] /= scale;
          dat[2] /= scale;
        }
    } /* for (i = 0; i < N - 2; ++i) */

  scale = fabs(dat[0]) + fabs(dat[1]);
  if (scale != 0.0)
    {
      /* scale to prevent overflow or underflow */
      dat[0] /= scale;
      dat[1] /= scale;
    }

  tau_i = gsl_linalg_householder_transform(&v2.vector);

  if (w->compute_t)
    {
      m = gsl_matrix_submatrix(w->H,
                               top + N - 2,
                               top + N - 3,
                               2,
                               w->size - top - N + 3);
      gsl_linalg_householder_hm(tau_i, &v2.vector, &m.matrix);

      m = gsl_matrix_submatrix(w->H,
                               0,
                               top + N - 2,
                               top + N,
                               2);
      gsl_linalg_householder_mh(tau_i, &v2.vector, &m.matrix);
    }
  else
    {
      m = gsl_matrix_submatrix(H, N - 2, N - 3, 2, 3);
      gsl_linalg_householder_hm(tau_i, &v2.vector, &m.matrix);

      m = gsl_matrix_submatrix(H, 0, N - 2, N, 2);
      gsl_linalg_householder_mh(tau_i, &v2.vector, &m.matrix);
    }

  if (w->Z)
    {
      /* accumulate transformation into Z */
      m = gsl_matrix_submatrix(w->Z, 0, top + N - 2, w->size, 2);
      gsl_linalg_householder_mh(tau_i, &v2.vector, &m.matrix);
    }

  return GSL_SUCCESS;
} /* francis_qrstep() */

/*
francis_search_subdiag_small_elements()
  Search for a small subdiagonal element starting from the bottom
of a matrix A. A small element is one that satisfies:

|A_{i,i-1}| <= eps * (|A_{i,i}| + |A_{i-1,i-1}|)

Inputs: A - matrix (must be at least 3-by-3)

Return: row index of small subdiagonal element or 0 if not found

Notes: the first small element that is found (starting from bottom)
       is set to zero
*/

static inline size_t
francis_search_subdiag_small_elements(gsl_matrix * A)
{
  const size_t N = A->size1;
  size_t i;
  double dpel = gsl_matrix_get(A, N - 2, N - 2);

  for (i = N - 1; i > 0; --i)
    {
      double sel = gsl_matrix_get(A, i, i - 1);
      double del = gsl_matrix_get(A, i, i);

      if ((sel == 0.0) ||
          (fabs(sel) < GSL_DBL_EPSILON * (fabs(del) + fabs(dpel))))
        {
          gsl_matrix_set(A, i, i - 1, 0.0);
          return (i);
        }

      dpel = del;
    }

  return (0);
} /* francis_search_subdiag_small_elements() */

/*
francis_schur_standardize()
  Convert a 2-by-2 diagonal block in the Schur form to standard form
and update the rest of T and Z matrices if required.

Inputs: A     - 2-by-2 matrix
        eval1 - where to store eigenvalue 1
        eval2 - where to store eigenvalue 2
        w     - francis workspace
*/

static inline void
francis_schur_standardize(gsl_matrix *A, gsl_complex *eval1,
                          gsl_complex *eval2,
                          gsl_eigen_francis_workspace *w)
{
  const size_t N = w->size;
  double cs, sn;
  size_t top;

  /*
   * figure out where the submatrix A resides in the
   * original matrix H
   */
  top = francis_get_submatrix(w->H, A);

  /* convert 2-by-2 block to standard form */
  francis_standard_form(A, &cs, &sn);

  /* set eigenvalues */

  GSL_SET_REAL(eval1, gsl_matrix_get(A, 0, 0));
  GSL_SET_REAL(eval2, gsl_matrix_get(A, 1, 1));
  if (gsl_matrix_get(A, 1, 0) == 0.0)
    {
      GSL_SET_IMAG(eval1, 0.0);
      GSL_SET_IMAG(eval2, 0.0);
    }
  else
    {
      double tmp = sqrt(fabs(gsl_matrix_get(A, 0, 1)) *
                        fabs(gsl_matrix_get(A, 1, 0)));
      GSL_SET_IMAG(eval1, tmp);
      GSL_SET_IMAG(eval2, -tmp);
    }

  if (w->compute_t)
    {
      gsl_vector_view xv, yv;

      /*
       * The above call to francis_standard_form transformed a 2-by-2 block
       * of T into upper triangular form via the transformation
       *
       * U = [ CS -SN ]
       *     [ SN  CS ]
       *
       * The original matrix T was
       *
       * T = [ T_{11} | T_{12} | T_{13} ]
       *     [   0*   |   A    | T_{23} ]
       *     [   0    |   0*   | T_{33} ]
       *
       * where 0* indicates all zeros except for possibly
       * one subdiagonal element next to A.
       *
       * After francis_standard_form, T looks like this:
       *
       * T = [ T_{11} | T_{12}  | T_{13} ]
       *     [   0*   | U^t A U | T_{23} ]
       *     [   0    |    0*   | T_{33} ]
       *
       * since only the 2-by-2 block of A was changed. However,
       * in order to be able to back transform T at the end,
       * we need to apply the U transformation to the rest
       * of the matrix T since there is no way to apply a
       * similarity transformation to T and change only the
       * middle 2-by-2 block. In other words, let
       *
       * M = [ I 0 0 ]
       *     [ 0 U 0 ]
       *     [ 0 0 I ]
       *
       * and compute
       *
       * M^t T M = [ T_{11} | T_{12} U |   T_{13}   ]
       *           [ U^t 0* | U^t A U  | U^t T_{23} ]
       *           [   0    |   0* U   |   T_{33}   ]
       *
       * So basically we need to apply the transformation U
       * to the i x 2 matrix T_{12} and the 2 x (n - i + 2)
       * matrix T_{23}, where i is the index of the top of A
       * in T.
       *
       * The BLAS routine drot() is suited for this.
       */

      if (top < (N - 2))
        {
          /* transform the 2 rows of T_{23} */

          xv = gsl_matrix_subrow(w->H, top, top + 2, N - top - 2);
          yv = gsl_matrix_subrow(w->H, top + 1, top + 2, N - top - 2);
          gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
        }

      if (top > 0)
        {
          /* transform the 2 columns of T_{12} */

          xv = gsl_matrix_subcolumn(w->H, top, 0, top);
          yv = gsl_matrix_subcolumn(w->H, top + 1, 0, top);
          gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
        }
    } /* if (w->compute_t) */

  if (w->Z)
    {
      gsl_vector_view xv, yv;

      /*
       * Accumulate the transformation in Z. Here, Z -> Z * M
       *
       * So:
       *
       * Z -> [ Z_{11} | Z_{12} U | Z_{13} ]
       *      [ Z_{21} | Z_{22} U | Z_{23} ]
       *      [ Z_{31} | Z_{32} U | Z_{33} ]
       *
       * So we just need to apply drot() to the 2 columns
       * starting at index 'top'
       */

      xv = gsl_matrix_column(w->Z, top);
      yv = gsl_matrix_column(w->Z, top + 1);

      gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
    } /* if (w->Z) */
} /* francis_schur_standardize() */

/*
francis_get_submatrix()
  B is a submatrix of A. The goal of this function is to
compute the indices in A of where the matrix B resides
*/

static inline size_t
francis_get_submatrix(gsl_matrix *A, gsl_matrix *B)
{
  size_t diff;
  double ratio;
  size_t top;

  diff = (size_t) (B->data - A->data);

  ratio = (double)diff / ((double) (A->tda + 1));

  top = (size_t) floor(ratio);

  return top;
} /* francis_get_submatrix() */

/*
francis_standard_form()
  Compute the Schur factorization of a real 2-by-2 matrix in
standard form:

[ A B ] = [ CS -SN ] [ T11 T12 ] [ CS SN ]
[ C D ]   [ SN  CS ] [ T21 T22 ] [-SN CS ]

where either:
1) T21 = 0 so that T11 and T22 are real eigenvalues of the matrix, or
2) T11 = T22 and T21*T12 < 0, so that T11 +/- sqrt(|T21*T12|) are
   complex conjugate eigenvalues

Inputs: A  - 2-by-2 matrix
        cs - where to store cosine parameter of rotation matrix
        sn - where to store sine parameter of rotation matrix

Notes: 1) based on LAPACK routine DLANV2
       2) On output, A is modified to contain the matrix in standard form
*/

static void
francis_standard_form(gsl_matrix *A, double *cs, double *sn)
{
  double a, b, c, d; /* input matrix values */
  double tmp;
  double p, z;
  double bcmax, bcmis, scale;
  double tau, sigma;
  double cs1, sn1;
  double aa, bb, cc, dd;
  double sab, sac;

  a = gsl_matrix_get(A, 0, 0);
  b = gsl_matrix_get(A, 0, 1);
  c = gsl_matrix_get(A, 1, 0);
  d = gsl_matrix_get(A, 1, 1);

  if (c == 0.0)
    {
      /*
       * matrix is already upper triangular - set rotation matrix
       * to the identity
       */
      *cs = 1.0;
      *sn = 0.0;
    }
  else if (b == 0.0)
    {
      /* swap rows and columns to make it upper triangular */

      *cs = 0.0;
      *sn = 1.0;

      tmp = d;
      d = a;
      a = tmp;
      b = -c;
      c = 0.0;
    }
  else if (((a - d) == 0.0) && (GSL_SIGN(b) != GSL_SIGN(c)))
    {
      /* the matrix has complex eigenvalues with a == d */
      *cs = 1.0;
      *sn = 0.0;
    }
  else
    {
      tmp = a - d;
      p = 0.5 * tmp;
      bcmax = GSL_MAX(fabs(b), fabs(c));
      bcmis = GSL_MIN(fabs(b), fabs(c)) * GSL_SIGN(b) * GSL_SIGN(c);
      scale = GSL_MAX(fabs(p), bcmax);
      z = (p / scale) * p + (bcmax / scale) * bcmis;

      if (z >= 4.0 * GSL_DBL_EPSILON)
        {
          /* real eigenvalues, compute a and d */

          z = p + GSL_SIGN(p) * fabs(sqrt(scale) * sqrt(z));
          a = d + z;
          d -= (bcmax / z) * bcmis;

          /* compute b and the rotation matrix */

          tau = gsl_hypot(c, z);
          *cs = z / tau;
          *sn = c / tau;
          b -= c;
          c = 0.0;
        }
      else
        {
          /*
           * complex eigenvalues, or real (almost) equal eigenvalues -
           * make diagonal elements equal
           */

          sigma = b + c;
          tau = gsl_hypot(sigma, tmp);
          *cs = sqrt(0.5 * (1.0 + fabs(sigma) / tau));
          *sn = -(p / (tau * (*cs))) * GSL_SIGN(sigma);

          /*
           * Compute [ AA BB ] = [ A B ] [ CS -SN ]
           *         [ CC DD ]   [ C D ] [ SN  CS ]
           */
          aa = a * (*cs) + b * (*sn);
          bb = -a * (*sn) + b * (*cs);
          cc = c * (*cs) + d * (*sn);
          dd = -c * (*sn) + d * (*cs);

          /*
           * Compute [ A B ] = [ CS SN ] [ AA BB ]
           *         [ C D ]   [-SN CS ] [ CC DD ]
           */
          a = aa * (*cs) + cc * (*sn);
          b = bb * (*cs) + dd * (*sn);
          c = -aa * (*sn) + cc * (*cs);
          d = -bb * (*sn) + dd * (*cs);

          tmp = 0.5 * (a + d);
          a = d = tmp;

          if (c != 0.0)
            {
              if (b != 0.0)
                {
                  if (GSL_SIGN(b) == GSL_SIGN(c))
                    {
                      /*
                       * real eigenvalues: reduce to upper triangular
                       * form
                       */
                      sab = sqrt(fabs(b));
                      sac = sqrt(fabs(c));
                      p = GSL_SIGN(c) * fabs(sab * sac);
                      tau = 1.0 / sqrt(fabs(b + c));
                      a = tmp + p;
                      d = tmp - p;
                      b -= c;
                      c = 0.0;

                      cs1 = sab * tau;
                      sn1 = sac * tau;
                      tmp = (*cs) * cs1 - (*sn) * sn1;
                      *sn = (*cs) * sn1 + (*sn) * cs1;
                      *cs = tmp;
                    }
                }
              else
                {
                  b = -c;
                  c = 0.0;
                  tmp = *cs;
                  *cs = -(*sn);
                  *sn = tmp;
                }
            }
        }
    }

  /* set new matrix elements */

  gsl_matrix_set(A, 0, 0, a);
  gsl_matrix_set(A, 0, 1, b);
  gsl_matrix_set(A, 1, 0, c);
  gsl_matrix_set(A, 1, 1, d);
} /* francis_standard_form() */


text:
/* specfunc/dilog.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_clausen.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_dilog.h>

/* Evaluate series for real dilog(x)
 * Sum[ x^k / k^2, {k,1,Infinity}]
 *
 * Converges rapidly for |x| < 1/2.
 */
static
int
dilog_series_1(const double x, gsl_sf_result * result)
{
  const int kmax = 1000;
  double sum  = x;
  double term = x;
  int k;
  for(k=2; k<kmax; k++) {
    const double rk = (k-1.0)/k;
    term *= x;
    term *= rk*rk;
    sum += term;
    if(fabs(term/sum) < GSL_DBL_EPSILON) break;
  }

  result->val  = sum;
  result->err  = 2.0 * fabs(term);
  result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

  if(k == kmax)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Compute the associated series
 *
 *   sum_{k=1}{infty} r^k / (k^2 (k+1))
 *
 * This is a series which appears in the one-step accelerated
 * method, which splits out one elementary function from the
 * full definition of Li_2(x). See below.
 */
static int
series_2(double r, gsl_sf_result * result)
{
  static const int kmax = 100;
  double rk = r;
  double sum = 0.5 * r;
  int k;
  for(k=2; k<10; k++)
  {
    double ds;
    rk *= r;
    ds = rk/(k*k*(k+1.0));
    sum += ds;
  }
  for(; k<kmax; k++)
  {
    double ds;
    rk *= r;
    ds = rk/(k*k*(k+1.0));
    sum += ds;
    if(fabs(ds/sum) < 0.5*GSL_DBL_EPSILON) break;
  }

  result->val = sum;
  result->err = 2.0 * kmax * GSL_DBL_EPSILON * fabs(sum);

  return GSL_SUCCESS;
}

/* Compute Li_2(x) using the accelerated series representation.
 *
 * Li_2(x) = 1 + (1-x)ln(1-x)/x + series_2(x)
 *
 * assumes: -1 < x < 1
 */
static int
dilog_series_2(double x, gsl_sf_result * result)
{
  const int stat_s3 = series_2(x, result);
  double t;
  if(x > 0.01)
    t = (1.0 - x) * log(1.0-x) / x;
  else
  {
    static const double c3 = 1.0/3.0;
    static const double c4 = 1.0/4.0;
    static const double c5 = 1.0/5.0;
    static const double c6 = 1.0/6.0;
    static const double c7 = 1.0/7.0;
    static const double c8 = 1.0/8.0;
    const double t68 = c6 + x*(c7 + x*c8);
    const double t38 = c3 + x *(c4 + x *(c5 + x * t68));
    t = (x - 1.0) * (1.0 + x*(0.5 + x*t38));
  }
  result->val += 1.0 + t;
  result->err += 2.0 * GSL_DBL_EPSILON * fabs(t);
  return stat_s3;
}

/* Calculates Li_2(x) for real x. Assumes x >= 0.0.
 */
static
int
dilog_xge0(const double x, gsl_sf_result * result)
{
  if(x > 2.0) {
    gsl_sf_result ser;
    const int stat_ser = dilog_series_2(1.0/x, &ser);
    const double log_x = log(x);
    const double t1 = M_PI*M_PI/3.0;
    const double t2 = ser.val;
    const double t3 = 0.5*log_x*log_x;
    result->val  = t1 - t2 - t3;
    result->err  = GSL_DBL_EPSILON * fabs(log_x) + ser.err;
    result->err += GSL_DBL_EPSILON * (fabs(t1) + fabs(t2) + fabs(t3));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_ser;
  }
  else if(x > 1.01) {
    gsl_sf_result ser;
    const int stat_ser = dilog_series_2(1.0 - 1.0/x, &ser);
    const double log_x    = log(x);
    const double log_term = log_x * (log(1.0-1.0/x) + 0.5*log_x);
    const double t1 = M_PI*M_PI/6.0;
    const double t2 = ser.val;
    const double t3 = log_term;
    result->val  = t1 + t2 - t3;
    result->err  = GSL_DBL_EPSILON * fabs(log_x) + ser.err;
    result->err += GSL_DBL_EPSILON * (fabs(t1) + fabs(t2) + fabs(t3));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_ser;
  }
  else if(x > 1.0) {
    /* series around x = 1.0 */
    const double eps = x - 1.0;
    const double lne = log(eps);
    const double c0 = M_PI*M_PI/6.0;
    const double c1 =   1.0 - lne;
    const double c2 = -(1.0 - 2.0*lne)/4.0;
    const double c3 =  (1.0 - 3.0*lne)/9.0;
    const double c4 = -(1.0 - 4.0*lne)/16.0;
    const double c5 =  (1.0 - 5.0*lne)/25.0;
    const double c6 = -(1.0 - 6.0*lne)/36.0;
    const double c7 =  (1.0 - 7.0*lne)/49.0;
    const double c8 = -(1.0 - 8.0*lne)/64.0;
    result->val = c0+eps*(c1+eps*(c2+eps*(c3+eps*(c4+eps*(c5+eps*(c6+eps*(c7+eps*c8)))))));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x == 1.0) {
    result->val = M_PI*M_PI/6.0;
    result->err = 2.0 * GSL_DBL_EPSILON * M_PI*M_PI/6.0;
    return GSL_SUCCESS;
  }
  else if(x > 0.5) {
    gsl_sf_result ser;
    const int stat_ser = dilog_series_2(1.0-x, &ser);
    const double log_x = log(x);
    const double t1 = M_PI*M_PI/6.0;
    const double t2 = ser.val;
    const double t3 = log_x*log(1.0-x);
    result->val  = t1 - t2 - t3;
    result->err  = GSL_DBL_EPSILON * fabs(log_x) + ser.err;
    result->err += GSL_DBL_EPSILON * (fabs(t1) + fabs(t2) + fabs(t3));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_ser;
  }
  else if(x > 0.25) {
    return dilog_series_2(x, result);
  }
  else if(x > 0.0) {
    return dilog_series_1(x, result);
  }
  else {
    /* x == 0.0 */
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
}

/* Evaluate the series representation for Li2(z):
 *
 *   Li2(z) = Sum[ |z|^k / k^2 Exp[i k arg(z)], {k,1,Infinity}]
 *   |z|    = r
 *   arg(z) = theta
 *
 * Assumes 0 < r < 1.
 * It is used only for small r.
 */
static
int
dilogc_series_1(
  const double r,
  const double x,
  const double y,
  gsl_sf_result * real_result,
  gsl_sf_result * imag_result
  )
{
  const double cos_theta = x/r;
  const double sin_theta = y/r;
  const double alpha = 1.0 - cos_theta;
  const double beta  = sin_theta;
  double ck = cos_theta;
  double sk = sin_theta;
  double rk = r;
  double real_sum = r*ck;
  double imag_sum = r*sk;
  const int kmax = 50 + (int)(22.0/(-log(r))); /* tuned for double-precision */
  int k;
  for(k=2; k<kmax; k++) {
    double dr, di;
    double ck_tmp = ck;
    ck = ck - (alpha*ck + beta*sk);
    sk = sk - (alpha*sk - beta*ck_tmp);
    rk *= r;
    dr = rk/((double)k*k) * ck;
    di = rk/((double)k*k) * sk;
    real_sum += dr;
    imag_sum += di;
    if(fabs((dr*dr + di*di)/(real_sum*real_sum + imag_sum*imag_sum)) < GSL_DBL_EPSILON*GSL_DBL_EPSILON) break;
  }

  real_result->val = real_sum;
  real_result->err = 2.0 * kmax * GSL_DBL_EPSILON * fabs(real_sum);
  imag_result->val = imag_sum;
  imag_result->err = 2.0 * kmax * GSL_DBL_EPSILON * fabs(imag_sum);

  return GSL_SUCCESS;
}

/* Compute
 *
 *   sum_{k=1}{infty} z^k / (k^2 (k+1))
 *
 * This is a series which appears in the one-step accelerated
 * method, which splits out one elementary function from the
 * full definition of Li_2.
 */
static int
series_2_c(
  double r,
  double x,
  double y,
  gsl_sf_result * sum_re,
  gsl_sf_result * sum_im
  )
{
  const double cos_theta = x/r;
  const double sin_theta = y/r;
  const double alpha = 1.0 - cos_theta;
  const double beta  = sin_theta;
  double ck = cos_theta;
  double sk = sin_theta;
  double rk = r;
  double real_sum = 0.5 * r*ck;
  double imag_sum = 0.5 * r*sk;
  const int kmax = 30 + (int)(18.0/(-log(r))); /* tuned for double-precision */
  int k;
  for(k=2; k<kmax; k++)
  {
    double dr, di;
    const double ck_tmp = ck;
    ck = ck - (alpha*ck + beta*sk);
    sk = sk - (alpha*sk - beta*ck_tmp);
    rk *= r;
    dr = rk/((double)k*k*(k+1.0)) * ck;
    di = rk/((double)k*k*(k+1.0)) * sk;
    real_sum += dr;
    imag_sum += di;
    if(fabs((dr*dr + di*di)/(real_sum*real_sum + imag_sum*imag_sum)) < GSL_DBL_EPSILON*GSL_DBL_EPSILON) break;
  }

  sum_re->val = real_sum;
  sum_re->err = 2.0 * kmax * GSL_DBL_EPSILON * fabs(real_sum);
  sum_im->val = imag_sum;
  sum_im->err = 2.0 * kmax * GSL_DBL_EPSILON * fabs(imag_sum);

  return GSL_SUCCESS;
}

/* Compute Li_2(z) using the one-step accelerated series.
 *
 * Li_2(z) = 1 + (1-z)ln(1-z)/z + series_2_c(z)
 *
 * z = r exp(i theta)
 * assumes: r < 1
 * assumes: r > epsilon, so that we take no special care with log(1-z)
 */
static
int
dilogc_series_2(
  const double r,
  const double x,
  const double y,
  gsl_sf_result * real_dl,
  gsl_sf_result * imag_dl
  )
{
  if(r == 0.0)
  {
    real_dl->val = 0.0;
    imag_dl->val = 0.0;
    real_dl->err = 0.0;
    imag_dl->err = 0.0;
    return GSL_SUCCESS;
  }
  else
  {
    gsl_sf_result sum_re;
    gsl_sf_result sum_im;
    const int stat_s3 = series_2_c(r, x, y, &sum_re, &sum_im);

    /* t = ln(1-z)/z */
    gsl_sf_result ln_omz_r;
    gsl_sf_result ln_omz_theta;
    const int stat_log = gsl_sf_complex_log_e(1.0-x, -y, &ln_omz_r, &ln_omz_theta);
    const double t_x = ( ln_omz_r.val * x + ln_omz_theta.val * y)/(r*r);
    const double t_y = (-ln_omz_r.val * y + ln_omz_theta.val * x)/(r*r);

    /* r = (1-z) ln(1-z)/z */
    const double r_x = (1.0 - x) * t_x + y * t_y;
    const double r_y = (1.0 - x) * t_y - y * t_x;

    real_dl->val = sum_re.val + r_x + 1.0;
    imag_dl->val = sum_im.val + r_y;
    real_dl->err = sum_re.err + 2.0*GSL_DBL_EPSILON*(fabs(real_dl->val) + fabs(r_x));
    imag_dl->err = sum_im.err + 2.0*GSL_DBL_EPSILON*(fabs(imag_dl->val) + fabs(r_y));
    return GSL_ERROR_SELECT_2(stat_s3, stat_log);
  }
}

/* Evaluate a series for Li_2(z) when |z| is near 1.
 * This is uniformly good away from z=1.
 *
 *   Li_2(z) = Sum[ a^n/n! H_n(theta), {n, 0, Infinity}]
 *
 * where
 *   H_n(theta) = Sum[ e^(i m theta) m^n / m^2, {m, 1, Infinity}]
 *   a = ln(r)
 *
 *  H_0(t) = Gl_2(t) + i Cl_2(t)
 *  H_1(t) = 1/2 ln(2(1-c)) + I atan2(-s, 1-c)
 *  H_2(t) = -1/2 + I/2 s/(1-c)
 *  H_3(t) = -1/2 /(1-c)
 *  H_4(t) = -I/2 s/(1-c)^2
 *  H_5(t) = 1/2 (2 + c)/(1-c)^2
 *  H_6(t) = I/2 s/(1-c)^5 (8(1-c) - s^2 (3 + c))
 */
static
int
dilogc_series_3(
  const double r,
  const double x,
  const double y,
  gsl_sf_result * real_result,
  gsl_sf_result * imag_result
  )
{
  const double theta = atan2(y, x);
  const double cos_theta = x/r;
  const double sin_theta = y/r;
  const double a = log(r);
  const double omc = 1.0 - cos_theta;
  const double omc2 = omc*omc;
  double H_re[7];
  double H_im[7];
  double an, nfact;
  double sum_re, sum_im;
  gsl_sf_result Him0;
  int n;

  H_re[0] = M_PI*M_PI/6.0 + 0.25*(theta*theta - 2.0*M_PI*fabs(theta));
  gsl_sf_clausen_e(theta, &Him0);
  H_im[0] = Him0.val;

  H_re[1] = -0.5*log(2.0*omc);
  H_im[1] = -atan2(-sin_theta, omc);

  H_re[2] = -0.5;
  H_im[2] = 0.5 * sin_theta/omc;

  H_re[3] = -0.5/omc;
  H_im[3] = 0.0;

  H_re[4] = 0.0;
  H_im[4] = -0.5*sin_theta/omc2;

  H_re[5] = 0.5 * (2.0 + cos_theta)/omc2;
  H_im[5] = 0.0;

  H_re[6] = 0.0;
  H_im[6] = 0.5 * sin_theta/(omc2*omc2*omc) * (8.0*omc - sin_theta*sin_theta*(3.0 + cos_theta));

  sum_re = H_re[0];
  sum_im = H_im[0];
  an = 1.0;
  nfact = 1.0;
  for(n=1; n<=6; n++) {
    double t;
    an *= a;
    nfact *= n;
    t = an/nfact;
    sum_re += t * H_re[n];
    sum_im += t * H_im[n];
  }

  real_result->val = sum_re;
  real_result->err = 2.0 * 6.0 * GSL_DBL_EPSILON * fabs(sum_re) + fabs(an/nfact);
  imag_result->val = sum_im;
  imag_result->err = 2.0 * 6.0 * GSL_DBL_EPSILON * fabs(sum_im) + Him0.err + fabs(an/nfact);

  return GSL_SUCCESS;
}

/* Calculate complex dilogarithm Li_2(z) in the fundamental region,
 * which we take to be the intersection of the unit disk with the
 * half-space x < MAGIC_SPLIT_VALUE. It turns out that 0.732 is a
 * nice choice for MAGIC_SPLIT_VALUE since then points mapped out
 * of the x > MAGIC_SPLIT_VALUE region and into another part of the
 * unit disk are bounded in radius by MAGIC_SPLIT_VALUE itself.
 *
 * If |z| < 0.98 we use a direct series summation. Otherwise z is very
 * near the unit circle, and the series_2 expansion is used; see above.
 * Because the fundamental region is bounded away from z = 1, this
 * works well.
 */
static
int
dilogc_fundamental(double r, double x, double y, gsl_sf_result * real_dl, gsl_sf_result * imag_dl)
{
  if(r > 0.98)
    return dilogc_series_3(r, x, y, real_dl, imag_dl);
  else if(r > 0.25)
    return dilogc_series_2(r, x, y, real_dl, imag_dl);
  else
    return dilogc_series_1(r, x, y, real_dl, imag_dl);
}

/* Compute Li_2(z) for z in the unit disk, |z| < 1. If z is outside
 * the fundamental region, which means that it is too close to z = 1,
 * then it is reflected into the fundamental region using the identity
 *
 *   Li2(z) = -Li2(1-z) + zeta(2) - ln(z) ln(1-z).
 */
static
int
dilogc_unitdisk(double x, double y, gsl_sf_result * real_dl, gsl_sf_result * imag_dl)
{
  static const double MAGIC_SPLIT_VALUE = 0.732;
  static const double zeta2 = M_PI*M_PI/6.0;
  const double r = hypot(x, y);

  if(x > MAGIC_SPLIT_VALUE)
  {
    /* Reflect away from z = 1 if we are too close. The magic value
     * insures that the reflected value of the radius satisfies the
     * related inequality r_tmp < MAGIC_SPLIT_VALUE.
     */
    const double x_tmp = 1.0 - x;
    const double y_tmp =     - y;
    const double r_tmp = hypot(x_tmp, y_tmp);
    /* const double cos_theta_tmp = x_tmp/r_tmp; */
    /* const double sin_theta_tmp = y_tmp/r_tmp; */

    gsl_sf_result result_re_tmp;
    gsl_sf_result result_im_tmp;

    const int stat_dilog = dilogc_fundamental(r_tmp, x_tmp, y_tmp, &result_re_tmp, &result_im_tmp);

    const double lnz    =  log(r);               /*  log(|z|)   */
    const double lnomz  =  log(r_tmp);           /*  log(|1-z|) */
    const double argz   =  atan2(y, x);          /*  arg(z) assuming principal branch */
    const double argomz =  atan2(y_tmp, x_tmp);  /*  arg(1-z)   */
    real_dl->val  = -result_re_tmp.val + zeta2 - lnz*lnomz + argz*argomz;
    real_dl->err  =  result_re_tmp.err;
    real_dl->err +=  2.0 * GSL_DBL_EPSILON * (zeta2 + fabs(lnz*lnomz) + fabs(argz*argomz));
    imag_dl->val  = -result_im_tmp.val - argz*lnomz - argomz*lnz;
    imag_dl->err  =  result_im_tmp.err;
    imag_dl->err +=  2.0 * GSL_DBL_EPSILON * (fabs(argz*lnomz) + fabs(argomz*lnz));

    return stat_dilog;
  }
  else
  {
    return dilogc_fundamental(r, x, y, real_dl, imag_dl);
  }
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_dilog_e(const double x, gsl_sf_result * result)
{
  if(x >= 0.0) {
    return dilog_xge0(x, result);
  }
  else {
    gsl_sf_result d1, d2;
    int stat_d1 = dilog_xge0( -x, &d1);
    int stat_d2 = dilog_xge0(x*x, &d2);
    result->val  = -d1.val + 0.5 * d2.val;
    result->err  =  d1.err + 0.5 * d2.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_d1, stat_d2);
  }
}

int
gsl_sf_complex_dilog_xy_e(
  const double x,
  const double y,
  gsl_sf_result * real_dl,
  gsl_sf_result * imag_dl
  )
{
  const double zeta2 = M_PI*M_PI/6.0;
  const double r2 = x*x + y*y;

  if(y == 0.0)
  {
    if(x >= 1.0)
    {
      imag_dl->val = -M_PI * log(x);
      imag_dl->err = 2.0 * GSL_DBL_EPSILON * fabs(imag_dl->val);
    }
    else
    {
      imag_dl->val = 0.0;
      imag_dl->err = 0.0;
    }
    return gsl_sf_dilog_e(x, real_dl);
  }
  else if(fabs(r2 - 1.0) < GSL_DBL_EPSILON)
  {
    /* Lewin A.2.4.1 and A.2.4.2 */

    const double theta = atan2(y, x);
    const double term1 = theta*theta/4.0;
    const double term2 = M_PI*fabs(theta)/2.0;
    real_dl->val = zeta2 + term1 - term2;
    real_dl->err = 2.0 * GSL_DBL_EPSILON * (zeta2 + term1 + term2);
    return gsl_sf_clausen_e(theta, imag_dl);
  }
  else if(r2 < 1.0)
  {
    return dilogc_unitdisk(x, y, real_dl, imag_dl);
  }
  else
  {
    /* Reduce argument to unit disk. */
    const double r = sqrt(r2);
    const double x_tmp =  x/r2;
    const double y_tmp = -y/r2;
    /* const double r_tmp = 1.0/r; */
    gsl_sf_result result_re_tmp, result_im_tmp;

    const int stat_dilog =
      dilogc_unitdisk(x_tmp, y_tmp, &result_re_tmp, &result_im_tmp);

    /* Unwind the inversion.
     *
     *  Li_2(z) + Li_2(1/z) = -zeta(2) - 1/2 ln(-z)^2
     */
    const double theta = atan2(y, x);
    const double theta_abs = fabs(theta);
    const double theta_sgn = ( theta < 0.0 ? -1.0 : 1.0 );
    const double ln_minusz_re = log(r);
    const double ln_minusz_im = theta_sgn * (theta_abs - M_PI);
    const double lmz2_re = ln_minusz_re*ln_minusz_re - ln_minusz_im*ln_minusz_im;
    const double lmz2_im = 2.0*ln_minusz_re*ln_minusz_im;
    real_dl->val = -result_re_tmp.val - 0.5 * lmz2_re - zeta2;
    real_dl->err =  result_re_tmp.err + 2.0*GSL_DBL_EPSILON*(0.5 * fabs(lmz2_re) + zeta2);
    imag_dl->val = -result_im_tmp.val - 0.5 * lmz2_im;
    imag_dl->err =  result_im_tmp.err + 2.0*GSL_DBL_EPSILON*fabs(lmz2_im);
    return stat_dilog;
  }
}

int
gsl_sf_complex_dilog_e(
  const double r,
  const double theta,
  gsl_sf_result * real_dl,
  gsl_sf_result * imag_dl
  )
{
  const double cos_theta = cos(theta);
  const double sin_theta = sin(theta);
  const double x = r * cos_theta;
  const double y = r * sin_theta;
  return gsl_sf_complex_dilog_xy_e(x, y, real_dl, imag_dl);
}

int
gsl_sf_complex_spence_xy_e(
  const double x,
  const double y,
  gsl_sf_result * real_sp,
  gsl_sf_result * imag_sp
  )
{
  const double oms_x = 1.0 - x;
  const double oms_y =     - y;
  return gsl_sf_complex_dilog_xy_e(oms_x, oms_y, real_sp, imag_sp);
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_dilog(const double x)
{
  EVAL_RESULT(gsl_sf_dilog_e(x, &result));
}

ext: c

max_stars_repo_name: ielomariala/Hex-Game

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"
#include "error_cblas_l2.h"

void
cblas_chemv (const enum CBLAS_ORDER order, const enum CBLAS_UPLO Uplo,
             const int N, const void *alpha, const void *A, const int lda,
             const void *X, const int incX, const void *beta, void *Y,
             const int incY)
{
#define BASE float
#include "source_hemv.h"
#undef BASE
}

max_stars_repo_licenses: Net-SNMP Xnet

max_issues_repo_licenses: Net-SNMP Xnet

max_forks_repo_licenses: Net-SNMP Xnet


text:
#pragma once
#include <cassert>
#include <gsl/span>

#include <iostream>

#include <optixu/optixpp_namespace.h>

namespace DeepestScatter
{
    template<typename T>
    class BufferBind
    {
    public:
        explicit BufferBind(optix::Buffer buffer, unsigned int level = 0);

        ~BufferBind();

        gsl::span<T>& getData();

        const T& operator [](size_t pos) const;
        T& operator [](size_t pos);

    private:
        optix::Buffer buffer;
        unsigned int level;

        gsl::span<T> optixOwned;
    };

    template<typename T>
    BufferBind<T>::BufferBind(optix::Buffer buffer, unsigned int level):
        buffer(buffer),
        level(level)
    {
        assert(sizeof(T) == buffer->getElementSize());
        auto rawArray = static_cast<T*>(buffer->map(level));

        RTsize totalSize = 1;
        RTsize sizes[3];
        buffer->getMipLevelSize(level, sizes[0], sizes[1], sizes[2]);
        for (unsigned int i = 0; i < buffer->getDimensionality(); i++)
        {
            totalSize *= sizes[i];
        }

        optixOwned = gsl::make_span(rawArray, totalSize);
    }

    template <typename T>
    BufferBind<T>::~BufferBind()
    {
        buffer->unmap(level);
    }

    template <typename T>
    gsl::span<T>& BufferBind<T>::getData()
    {
        return optixOwned;
    }

    template <typename T>
    inline const T& BufferBind<T>::operator[](size_t pos) const
    {
        return optixOwned[pos];
    }

    template <typename T>
    inline T& BufferBind<T>::operator[](size_t pos)
    {
        return optixOwned[pos];
    }
}

ext: h

max_stars_repo_name: marsermd/DeepestScatter

max_issues_repo_name: marsermd/DeepestScatter

max_forks_repo_name: marsermd/DeepestScatter


text:
/* multifit_nlinear/qr.c
 *
 * Copyright (C) 2015, 2016 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * This module handles the solution of the linear least squares
 * system:
 *
 * [     J~      ] p~ = - [ f ]
 * [ sqrt(mu)*D~ ]        [ 0 ]
 *
 * using a QR approach. Quantities are scaled according to:
 *
 * J~ = J S
 * D~ = D S
 * p~ = S^{-1} p
 *
 * where S is a diagonal matrix and S_jj = || J_j || and J_j is column
 * j of the Jacobian. This balancing transformation seems to be more
 * numerically stable for some Jacobians.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

#include "common.c"
#include "qrsolv.c"

typedef struct
{
  size_t p;
  gsl_matrix *QR;            /* QR factorization of J */
  gsl_vector *tau_Q;         /* Householder scalars for Q */
  gsl_matrix *T;             /* workspace matrix for qrsolv, p-by-p */
  gsl_permutation *perm;     /* permutation matrix */
  size_t rank;               /* rank of J */
  gsl_vector *residual;      /* residual of LS problem [ J; sqrt(mu) D ] p = - [ f; 0 ] */
  gsl_vector *qtf;           /* Q^T f */
  gsl_vector *workn;         /* workspace, length n */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *work3p;        /* workspace, length 3*p */
  double mu;                 /* LM parameter */
} qr_state_t;

static int qr_init(const void * vtrust_state, void * vstate);
static int qr_presolve(const double mu, const void * vtrust_state, void * vstate);
static int qr_solve(const gsl_vector * f, gsl_vector *x,
                    const void * vtrust_state, void *vstate);
static int qr_rcond(double * rcond, void * vstate);

static void *
qr_alloc (const size_t n, const size_t p)
{
  qr_state_t *state;

  (void)n;

  state = calloc(1, sizeof(qr_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate qr state", GSL_ENOMEM);
    }

  state->QR = gsl_matrix_alloc(n, p);
  if (state->QR == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for QR", GSL_ENOMEM);
    }

  state->tau_Q = gsl_vector_alloc(p);
  if (state->tau_Q == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for tau_Q",
                      GSL_ENOMEM);
    }

  state->T = gsl_matrix_alloc(p, p);
  if (state->T == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for T", GSL_ENOMEM);
    }

  state->qtf = gsl_vector_alloc(n);
  if (state->qtf == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for qtf",
                      GSL_ENOMEM);
    }

  state->residual = gsl_vector_alloc(n);
  if (state->residual == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for residual",
                      GSL_ENOMEM);
    }

  state->perm = gsl_permutation_calloc(p);
  if (state->perm == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for perm",
                      GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn",
                      GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp",
                      GSL_ENOMEM);
    }

  state->work3p = gsl_vector_alloc(3 * p);
  if (state->work3p == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for work3p",
                      GSL_ENOMEM);
    }

  state->p = p;
  state->mu = 0.0;
  state->rank = 0;

  return state;
}

static void
qr_free(void *vstate)
{
  qr_state_t *state = (qr_state_t *) vstate;

  if (state->QR)
    gsl_matrix_free(state->QR);

  if (state->tau_Q)
    gsl_vector_free(state->tau_Q);

  if (state->T)
    gsl_matrix_free(state->T);

  if (state->qtf)
    gsl_vector_free(state->qtf);

  if (state->residual)
    gsl_vector_free(state->residual);

  if (state->perm)
    gsl_permutation_free(state->perm);

  if (state->workn)
    gsl_vector_free(state->workn);

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->work3p)
    gsl_vector_free(state->work3p);

  free(state);
}

/* compute J = Q R PT */
static int
qr_init(const void * vtrust_state, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  qr_state_t *state = (qr_state_t *) vstate;
  int signum;

  /* perform QR decomposition of J */
  gsl_matrix_memcpy(state->QR, trust_state->J);
  gsl_linalg_QRPT_decomp(state->QR, state->tau_Q, state->perm,
                         &signum, state->workp);

  return GSL_SUCCESS;
}

static int
qr_presolve(const double mu, const void * vtrust_state, void * vstate)
{
  qr_state_t *state = (qr_state_t *) vstate;

  state->mu = mu;

  (void) vtrust_state;

  return GSL_SUCCESS;
}

static int
qr_solve(const gsl_vector * f, gsl_vector *x,
         const void * vtrust_state, void *vstate)
{
  qr_state_t *state = (qr_state_t *) vstate;
  int status;

  if (state->mu == 0.0)
    {
      /*
       * compute Gauss-Newton direction by solving
       * J x = f
       * with an attempt to identify rank deficiency in J
       */
      size_t rank = gsl_linalg_QRPT_rank(state->QR, -1.0);
      status = gsl_linalg_QRPT_lssolve2(state->QR, state->tau_Q, state->perm,
                                        f, rank, x, state->residual);
    }
  else
    {
      /*
       * solve:
       *
       * [     J      ] x = [ f ]
       * [ sqrt(mu) D ]     [ 0 ]
       *
       * using QRPT factorization of J
       */

      const gsl_multifit_nlinear_trust_state *trust_state =
        (const gsl_multifit_nlinear_trust_state *) vtrust_state;
      double sqrt_mu = sqrt(state->mu);

      /* compute qtf = Q^T f */
      gsl_vector_memcpy(state->qtf, f);
      gsl_linalg_QR_QTvec(state->QR, state->tau_Q, state->qtf);

      status = qrsolv(state->QR, state->perm, sqrt_mu, trust_state->diag,
                      state->qtf, state->T, x, state->workn);
    }

  /* reverse step to go downhill */
  gsl_vector_scale(x, -1.0);

  return status;
}

static int
qr_rcond(double * rcond, void * vstate)
{
  int status;
  qr_state_t *state = (qr_state_t *) vstate;

  status = gsl_linalg_QRPT_rcond(state->QR, rcond, state->work3p);

  return status;
}

static const gsl_multifit_nlinear_solver qr_type =
{
  "qr",
  qr_alloc,
  qr_init,
  qr_presolve,
  qr_solve,
  qr_rcond,
  qr_free
};

const gsl_multifit_nlinear_solver *gsl_multifit_nlinear_solver_qr = &qr_type;


text:
#ifndef LASER_SCANNER_H_WSZJIY40
#define LASER_SCANNER_H_WSZJIY40

#include <cmath>
#include <gsl/gsl>
#include <sens_loc/camera_models/concepts.h>
#include <sens_loc/math/constants.h>
#include <sens_loc/math/coordinate.h>
#include <sens_loc/math/scaling.h>
#include <stdexcept>
#include <type_traits>

namespace sens_loc::camera_models {

namespace detail {
template <typename Real>
Real get_d_phi(int width) noexcept {
    static_assert(std::is_floating_point_v<Real>);
    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
    return Real(2.) * math::pi<Real> / Real(width);
}
}  // namespace detail

/// This struct contains the parameters for describing the equirectangular
/// projection of a laser scan.
///
/// The scan is expected to be 360° in horizontal direction and som
/// varying degree in vertical direction.
/// The conversions and projections are mainly conversion from a
/// spherical coordinate-system to a cartesian.
/// \tparam Real precision of the parameters and calculations.
///
/// \note the coordinate convention for spherical coordinates is as follows:
///   - \f$\varphi \in [0, 2 \pi] \mapsto u\f$ note that this wraps around
///   - \f$\theta \in [0, \pi] \mapsto v\f$ - not wrapping!
///   - it is possible to have a different range for \f$\theta\f$ but not for
///     \f$\varphi\f$.
///
/// \tparam Real floating point type that determines the precision of the
/// calculations.
/// \sa is_intrinsic_v
template <typename Real = float>
class equirectangular {
  public:
    static_assert(std::is_floating_point_v<Real>);
    using real_type = Real;

    /// Default initialize all parameters to zero.
    equirectangular() = default;

    /// Construct a equirectangular image for the full sphere.
    ///
    /// \param width width of the image, this maps to 360° field of vie
    /// horizontally which wraps around
    /// \param height height of the image, this
    /// maps to 180° field of view vertically which does __NOT__ wrap
    equirectangular(int width, int height) noexcept
        : _w(width)
        , _h(height)
        , d_phi(detail::get_d_phi<Real>(width))
        , d_theta(math::pi<Real> / Real(height))
        , theta_min(Real(0.)) {
        Expects(width > 0);
        Expects(height > 0);
        ensure_invariant();
    }

    /// Construct the model with a custom \f$\theta\f$-range. This model does
    /// __NOT__ map to the whole sphere, but is not a cylindrical coordinate
    /// system as well!
    /// \param width,height image dimensions
    /// \param theta_range minimum and maximum angle on the unit-sphere in
    /// vertical direction.
    equirectangular(int                       width,
                    int                       height,
                    math::numeric_range<Real> theta_range) noexcept
        : _w(width)
        , _h(height)
        , d_phi(detail::get_d_phi<Real>(width))
        , d_theta((theta_range.max - theta_range.min) / Real(height))
        , theta_min(theta_range.min) {
        Expects(width > 0);
        Expects(height > 0);
        Expects(theta_range.min >= 0.);
        Expects(theta_range.max <= math::pi<Real>);
        ensure_invariant();
    }

    /// Construct the model with a minimum angle \p theta_min and an angle
    /// increment \p d_theta.
    ///
    /// \param width,height image dimensions
    /// \param theta_min,d_theta vertical resolution configuration
    /// \throws if the \f$\theta\f$-angle would be out of the range with the
    /// configuration this constructor throws an exception.
    equirectangular(int width, int height, Real theta_min, Real d_theta)
        : _w(width)
        , _h(height)
        , d_phi(detail::get_d_phi<Real>(width))
        , d_theta(d_theta)
        , theta_min(theta_min) {
        const Real theta_max = theta_min + height * d_theta;
        if (theta_max > math::pi<Real>)
            throw std::invalid_argument("angle increment too big");
        ensure_invariant();
    }

    /// Return the width of the image corresponding to this intrinsic.
    [[nodiscard]] int w() const noexcept { return _w; }
    /// Return the height of the image corresponding to this intrinsic.
    [[nodiscard]] int h() const noexcept { return _h; }

    /// This methods calculates the inverse projection of the equirectangular
    /// model to get the direction of the lightray for the pixel at \p p.
    ///
    /// \tparam _Real either integer pixels or floating point for subpixels
    /// \param p non-negative pixel coordinates
    /// \post \f$\lVert result \rVert_2 = 1.\f$
    /// \returns normalized vector in camera coordinates - unit sphere
    /// coordinate
    /// \note if \p _Real is an integer-type the value is itself backprojected,
    /// which usually means the bottom left corner of the pixel and __NOT__
    /// its center!
    template <typename _Real = int>
    [[nodiscard]] math::sphere_coord<Real>
    pixel_to_sphere(const math::pixel_coord<_Real>& p) const noexcept;

    /// Project points in camera coordinates to pixel coordinates.
    /// \note if the point can not be projected (coordinate not in view) the
    /// pixel coordinate {-1, -1} is returned.
    /// \sa equirectangular::project_to_sphere
    template <typename _Real = Real>
    [[nodiscard]] math::pixel_coord<_Real>
    camera_to_pixel(const math::camera_coord<Real>& p) const noexcept;

  private:
    void ensure_invariant() const noexcept {
        Ensures(d_phi > Real(0.));
        Ensures(d_theta > Real(0.));
        Ensures(theta_min >= Real(0.));

        // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
        Ensures(std::abs(d_phi * _w - Real(2.) * math::pi<Real>) < 0.00001);
        // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
        Ensures(theta_min + _h * d_theta <= math::pi<Real> + 0.00001);
    }

    int  _w        = 0;   ///< width of a laser-scan image
    int  _h        = 0;   ///< height of a laser-scan image.
    Real d_phi     = 0.;  ///< Angle increment in u-direction.
    Real d_theta   = 0.;  ///< Angle increment in v-direction.
    Real theta_min = 0.;  ///< Smallest angle in v-direction.
};

template <typename Real>
template <typename _Real>
inline math::sphere_coord<Real>
equirectangular<Real>::pixel_to_sphere(const math::pixel_coord<_Real>& p) const
    noexcept {
    static_assert(std::is_arithmetic_v<_Real>);
    Expects(p.u() >= Real(0.0));
    Expects(p.v() < Real(w()));
    Expects(p.u() >= Real(0.0));
    Expects(p.v() < Real(h()));

    const Real phi   = p.u() * d_phi - math::pi<Real>;
    const Real theta = theta_min + (p.v() * d_theta);

    Ensures(phi >= -math::pi<Real>);
    Ensures(phi <= math::pi<Real>);
    Ensures(theta >= Real(0.));
    Ensures(theta <= math::pi<Real>);

    using std::cos;
    using std::sin;
    math::sphere_coord<Real> s{sin(theta) * cos(phi), sin(theta) * sin(phi),
                               cos(theta)};
    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
    Ensures(std::abs(s.norm() - Real(1.0)) < 0.000001);
    return s;
}

template <typename Real>
template <typename _Real>
inline math::pixel_coord<_Real>
equirectangular<Real>::camera_to_pixel(const math::camera_coord<Real>& p) const
    noexcept {
    static_assert(std::is_arithmetic_v<_Real>);

    const Real r = std::sqrt(p.X() * p.X() + p.Y() * p.Y() + p.Z() * p.Z());
    if (r == Real(0.0))
        return {_Real(-1), _Real(-1)};

    const Real theta = std::acos(p.Z() / r);
    const Real phi   = std::atan2(p.Y(), p.X());

    Ensures(r >= Real(0.0));
    Ensures(phi >= -math::pi<Real>);
    Ensures(phi <= math::pi<Real>);
    Ensures(theta >= Real(0.));
    Ensures(theta <= math::pi<Real>);

    const _Real u = gsl::narrow_cast<_Real>((phi + math::pi<Real>) / d_phi);
    const _Real v = gsl::narrow_cast<_Real>(theta / d_theta);

    if (u < _Real(0.0) || u > gsl::narrow_cast<Real>(w()) || v < _Real(0.0) ||
        v > gsl::narrow_cast<Real>(h()))
        return {_Real(-1), _Real(-1)};

    return {u, v};
}
}  // namespace sens_loc::camera_models

#endif /* end of include guard: LASER_SCANNER_H_WSZJIY40 */

max_stars_repo_name: JonasToth/depth-conversions

max_issues_repo_name: JonasToth/depth-conversions

max_forks_repo_name: JonasToth/depth-conversions


text:
#pragma once
//=============================================================================
// EXTERNAL DECLARATIONS
//=============================================================================
#include <Core/Components/IdType.h>
#include "Core/Game/IManager.h"
#include <gsl/span>
#include <memory>

namespace engine
{
//=============================================================================
// FORWARD DECLARATIONS
//=============================================================================
class IGameEngine;
using GameEngineRef = std::shared_ptr<IGameEngine>;
using GameEngineWeakRef = std::weak_ptr<IGameEngine>;

//=============================================================================
// INTERFACE IManagerComponent
//=============================================================================
class IManagerComponent
{
public:
	virtual ~IManagerComponent() {}

public:
	virtual gsl::span<IdType> interfaces() = 0;
	virtual void onAttached(const GameEngineRef &iGameEngine) = 0;
	virtual void onDetached(const GameEngineRef &iGameEngine) = 0;
};
} // namespace engine


text:
/*
 * Created on: Mar 22, 2016
 * Author: Steffen Rechner <steffen.rechner@informatik.uni-halle.de>
 *
 * This file is part of the marathon software.
 *
 * Copyright (c) 2016, Steffen Rechner
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is furnished
 * to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef INCLUDE_MARATHON_TRANSITIONMATRIX_H_
#define INCLUDE_MARATHON_TRANSITIONMATRIX_H_

#include "state_graph.h"

#ifdef USE_ARMADILLO

#include <armadillo>

#endif

#ifdef USE_BLAS

#include <cblas.h>

#endif

namespace marathon {

    /**
     * Virtual Base Class for Transition Matrix.
     */
    template<class T=double>
    class TransitionMatrix {

    protected:

        size_t N;               // number of rows and columns
        size_t ld;              // lead dimension (upper bound on n)
        std::vector<T> data;    // actual data array

    public:

        /**
         * Standard Constructor. Create uninitialized transition matrix of size N times N.
         * @param N number of rows or columns
         */
        TransitionMatrix(const size_t N) :
                N(N),
                ld(((N + 255) / 256) * 256) // lead dimension is next mulitple of 256
        {
            data.resize(N * ld, 0);
        }

        /**
         * Constructor. Create Transition Matrix from State Graph.
         * @param sg Pointer to state graph object.
         */
        TransitionMatrix(const StateGraph &sg) :
                TransitionMatrix(sg.getNumStates()) {

            for (const Transition *t : sg.getArcs()) {
                this->data[t->from * ld + t->to] = t->weight.convert_to<T>();
            }
        }

        /**
         *  Return size of the matrix.
         */
        size_t getDimension() const {
            return N;
        }

        /**
         * Return lead dimension of the matrix.
         */
        size_t getLeadDimension() const {
            return ld;
        }

        /**
         * Return a pointer to the data.
         */
        const std::vector<T> &getData() const {
            return data;
        }

        /**
         * Return P[i,j].
         * @param i row index
         * @param j column index
         * @return P[i,j]
         */
        T get(size_t i, size_t j) const {
            return data[i * ld + j];
        }

        /**
         * Set P[i,j] to x.
         * @param i row index.
         * @param j column index.
         * @param x value of type T
         */
        void set(size_t i, size_t j, T x) {
            data[i * ld + j] = x;
        }

        /**
         * Overwrite the current matrix with zeroes.
         */
        virtual void clear() {
            data.resize(N * ld, T(0));
        }

        /**
         * Compute P^k.
         * @param P A pointer to a Transition Matrix.
         * @param k Exponent.
         * @return P^k
         */
        TransitionMatrix<T> pow(uint k) const {

            // init matrix
            if (k == 0) {
                return eye(N);
            }

            // create binary representation of k
            int bin[32];
            memset(bin, 0, 32 * sizeof(int));
            int l = 31;
            int kk = k;
            while (kk > 0) {
                bin[l] = kk % 2;
                kk >>= 1;
                l--;
            }
            l += 2;

#ifdef DEBUG
            std::cout << "bin: ";
            for (int i = 0; i < 32; i++) {
                std::cout << bin[i];
            }
            std::cout << " l=" << l << std::endl;
#endif

            TransitionMatrix<T> A(*this); // will be returned

            // binary exponentation - Left to Right (see Don. Knuth: Seminumerical Alg. Vol. 2 page 461)
            while (l < 32) {

                // square
                A = A * A;

                // multiply
                if (bin[l] == 1)
                    A = A * *this;

                l++;
            }

            return A;
        }

        /**
         * Matrix multiplication.
         * @param P Transition matrix.
         * @return P * this
         */
        TransitionMatrix<T> operator*(const TransitionMatrix<T> &P) const {

            TransitionMatrix<T> X(N);  // will be returned

#pragma omp parallel for
            for (size_t i = 0; i < N; i++) {
                for (size_t j = 0; j < N; j++) {
                    T p_ij = 0;
                    for (size_t k = 0; k < N; k++) {
                        p_ij += this->get(i, k) * P.get(k, j);
                    }
                    X.set(i, j, p_ij);
                }
            }

            return X;
        }

        /**
         * Return a string that represents the matrix.
         */
        virtual std::string to_string() const {

            std::stringstream ss;
            ss << "\n";
            for (size_t i = 0; i < this->N; i++) {
                ss << "  ";
                for (size_t j = 0; j < this->N - 1; j++) {
                    ss << std::setprecision(std::numeric_limits<T>::digits10) << std::fixed
                       << this->data[i * this->ld + j] << "  ";
                }
                ss << std::setprecision(std::numeric_limits<T>::digits10) << std::fixed
                   << this->data[i * this->ld + this->N - 1];
                ss << "\n";
            }

            return ss.str();

        }

        /**
         * To output into streams.
         */
        friend inline std::ostream &operator<<(std::ostream &out,
                                               const TransitionMatrix<T> &s) {
            out << s.to_string();
            return out;
        }

        /**
         * Return the identity matrix with N rows and columns.
         * @param N Number of rows and columns.
         * @return Identity matrix.
         */
        static TransitionMatrix<T> eye(size_t N) {
            TransitionMatrix<T> P(N);
            for (size_t i = 0; i < N; i++)
                P.set(i,i,1);
            return P;
        }

    };

    /***********************************************************************
     * template specializations
     **********************************************************************/

#ifdef USE_BLAS

    template<>
    TransitionMatrix<float> TransitionMatrix<float>::operator*(const TransitionMatrix<float> &P) const {

        const float alpha = 1.0;
        const float beta = 0.0;

        TransitionMatrix<float> X(N);

        // use cblas
        cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, N, N, N, alpha,
                    &P.data[0], P.ld, &data[0], ld, beta, &X.data[0], X.ld);

        return X;
    }

    template<>
    TransitionMatrix<double> TransitionMatrix<double>::operator*(const TransitionMatrix<double> &P) const {

        const double alpha = 1.0;
        const double beta = 0.0;

        TransitionMatrix<double> X(N);

        // use cblas
        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, N, N, N, alpha,
                    &P.data[0], P.ld, &data[0], ld, beta, &X.data[0], X.ld);

        return X;
    }

#endif

}

#endif /* INCLUDE_MARATHON_TRANSITIONMATRIX_H_ */

max_stars_repo_name: srechner/marathonR

max_issues_repo_name: srechner/marathonR

max_forks_repo_name: srechner/marathonR


text:
/**
 * @copyright (c) 2017 King Abdullah University of Science and Technology (KAUST).
 *                     All rights reserved.
 **/
/**
 * @file auxcompute_z.h
 *
 * This file contains the declarations of computational auxiliary functions.
 *
 * HiCMA is a software package provided by King Abdullah University of Science and Technology (KAUST)
 *
 * @version 0.1.1
 * @author Kadir Akbudak
 * @date 2018-11-08
 **/
#ifndef __AUXCOMPUTE_Z__
#define __AUXCOMPUTE_Z__

#include "hicma_struct.h"
#ifdef MKL
  #include <mkl.h>
  //#pragma message("MKL is used")
#else
  #include <cblas.h>
  #ifdef LAPACKE_UTILS
    #include <lapacke_utils.h>
  #endif
  #include <lapacke.h>
  //#pragma message("MKL is NOT used")
#endif

#include <stdio.h>
#include "morse.h"
#ifndef min
#define min(a, b) ((a) < (b) ? (a) : (b))
#endif

#include "starsh.h"
int HICMA_zuncompress(
        MORSE_enum uplo, MORSE_desc_t *AUV, MORSE_desc_t *AD, MORSE_desc_t *Ark);
int HICMA_zuncompress_custom_size(MORSE_enum uplo,
        MORSE_desc_t *AUV, MORSE_desc_t *AD, MORSE_desc_t *Ark,
        int numrows_matrix,
        int numcolumns_matrix,
        int numrows_block,
        int numcolumns_block
        );
int HICMA_zdiag_vec2mat(
        MORSE_desc_t *vec, MORSE_desc_t *mat);
void HICMA_znormest( int M, int N, double *A, double *e, double *work);
void HICMA_zgenerate_problem(
        int probtype, //problem type defined in hicma_constants.h
        char sym,     // symmetricity of problem: 'N' or 'S'
        double decay, // decay of singular values. Will be used in HICMA_STARSH_PROB_RND. Set 0 for now.
        int _M,       // number of rows/columns of matrix
        int _nb,      // number of rows/columns of a single tile
        int _mt,      // number of tiles in row dimension
        int _nt,      // number of tiles in column dimension
        HICMA_problem_t *hicma_problem // pointer to hicma struct (starsh format will be used to pass coordinate info to number generation and compression phase)
        );
#endif

max_stars_repo_name: Quansight/hicma

max_issues_repo_name: Quansight/hicma