text:
#ifndef TNet_Matrix_h
#define TNet_Matrix_h

#include <stddef.h>
#include <stdlib.h>
#include <stdexcept>
#include <iostream>

#ifdef HAVE_ATLAS
extern "C"{
  #include <cblas.h>
  #include <clapack.h>
}
#endif

#include "Common.h"
#include "MathAux.h"
#include "Types.h"
#include "Error.h"

//#define TRACE_MATRIX_OPERATIONS
#define CHECKSIZE

namespace TNet
{

  //  class matrix_error : public std::logic_error {};
  //  class matrix_sizes_error : public matrix_error {};

  // declare the class so the header knows about it
  template<typename _ElemT> class Vector;
  template<typename _ElemT> class SubVector;
  template<typename _ElemT> class Matrix;
  template<typename _ElemT> class SubMatrix;

  // we need to declare the friend << operator here
  template<typename _ElemT>
    std::ostream & operator << (std::ostream & rOut, const Matrix<_ElemT> & rM);

  // we need to declare the friend << operator here
  template<typename _ElemT>
    std::istream & operator >> (std::istream & rIn, Matrix<_ElemT> & rM);

  // we need to declare this friend function here
  template<typename _ElemT>
   _ElemT TraceOfProduct(const Matrix<_ElemT> &A, const Matrix<_ElemT> &B); // tr(A B)

  // we need to declare this friend function here
  template<typename _ElemT>
	 _ElemT TraceOfProductT(const Matrix<_ElemT> &A, const Matrix<_ElemT> &B); // tr(A B^T)==tr(A^T B)

  /** **************************************************************************
   ** **************************************************************************
   *  @brief Provides a matrix class
   *
   *  This class provides a way to work with matrices in TNet.
   *  It encapsulates basic operations and memory optimizations.
   *
   */
  template<typename _ElemT>
    class Matrix
    {
    public:
      /// defines a transpose type

      struct HtkHeader
      {
        INT_32    mNSamples;
        INT_32    mSamplePeriod;
        INT_16    mSampleSize;
        UINT_16   mSampleKind;
      };

      /**
       * @brief Extension of the HTK header
       */
      struct HtkHeaderExt
      {
        INT_32 mHeaderSize;
        INT_32 mVersion;
        INT_32 mSampSize;
      };

      /// defines a type of this
      typedef Matrix<_ElemT>    ThisType;

      // Constructors

      /// Empty constructor
      Matrix<_ElemT> ():
        mpData(NULL), mMCols(0), mMRows(0), mStride(0)
#ifdef STK_MEMALIGN_MANUAL
        , mpFreeData(NULL)
#endif
      {}

      /// Copy constructor
      Matrix<_ElemT> (const Matrix<_ElemT> & rM, MatrixTrasposeType trans=NO_TRANS):
        mpData(NULL)
      { if(trans==NO_TRANS){ Init(rM.mMRows, rM.mMCols); Copy(rM); } else { Init(rM.mMCols,rM.mMRows); Copy(rM,TRANS); } }

      /// Copy constructor from another type.
      template<typename _ElemU>
      explicit Matrix<_ElemT> (const Matrix<_ElemU> & rM, MatrixTrasposeType trans=NO_TRANS):
        mpData(NULL)
      { if(trans==NO_TRANS){ Init(rM.Rows(), rM.Cols()); Copy(rM); } else { Init(rM.Cols(),rM.Rows()); Copy(rM,TRANS); } }

      /// Basic constructor
      Matrix(const size_t r, const size_t c, bool clear=true)
      { mpData=NULL; Init(r, c, clear);  }

	  Matrix<_ElemT> &operator = (const Matrix <_ElemT> &other) { Init(other.Rows(), other.Cols()); Copy(other); return *this; } // Needed for inclusion in std::vector

      /// Destructor
      ~Matrix()
      { Destroy(); }

      /// Initializes matrix (if not done by constructor)
      ThisType &
      Init(const size_t r,
           const size_t c, bool clear=true);

      /**
       * @brief Dealocates the matrix from memory and resets the dimensions to (0, 0)
       */
      void
      Destroy();

      ThisType &
      Zero();

      ThisType &
      Unit(); // set to unit.

      /**
       * @brief Copies the contents of a matrix
       * @param rM Source data matrix
       * @return Returns reference to this
       */
      template<typename _ElemU> ThisType &
        Copy(const Matrix<_ElemU> & rM, MatrixTrasposeType Trans=NO_TRANS);

      /**
       * @brief Copies the elements of a vector row-by-row into a matrix
       * @param rV Source vector
       * @param nRows Number of rows of returned matrix
       * @param nCols Number of columns of returned matrix
       *
       * Note that rV.Dim() must equal nRows*nCols
       */
      ThisType &
      CopyVectorSplicedRows(const Vector<_ElemT> &rV, const size_t nRows, const size_t nCols);

      /**
       * @brief Returns @c true if matrix is initialized
       */
      bool
		IsInitialized() const
      { return mpData != NULL; }

      /// Returns number of rows in the matrix
      inline size_t
		Rows() const
      {
        return mMRows;
      }

      /// Returns number of columns in the matrix
      inline size_t
      Cols() const
      {
        return mMCols;
      }

      /// Returns number of columns in the matrix memory
      inline size_t
		Stride() const
      {
        return mStride;
      }

      /**
       *  @brief Gives access to a specified matrix row without range check
       *  @return Pointer to the const array
       */
      inline const _ElemT*  __attribute__((aligned(16)))
       pData () const
      {
        return mpData;
      }

      /**
       *  @brief Gives access to a specified matrix row without range check
       *  @return Pointer to the non-const data array
       */
      inline _ElemT* __attribute__((aligned(16)))
       pData ()
      {
        return mpData;
      }

      /**
       *  @brief pData_workaround is a workaround that allows SubMatrix to get a
       *  @return pointer to non-const data even though the Matrix is const...
       */
    protected:
      inline _ElemT*  __attribute__((aligned(16)))
       pData_workaround () const
      {
        return mpData;
      }
    public:

      /// Returns size of matrix in memory
      size_t
      MSize() const
      {
        return mMRows * mStride * sizeof(_ElemT);
      }

      /// Checks the content of the matrix for nan and inf values
      void
      CheckData(const std::string file = "") const
      {
        for(size_t row=0; row<Rows(); row++) {
          for(size_t col=0; col<Cols(); col++) {
            if(isnan((*this)(row,col)) || isinf((*this)(row,col))) {
              std::ostringstream os;
              os << "Invalid value: " << (*this)(row,col)
                 << " in matrix row: " << row
                 << " col: " << col
                 << " file: " << file;
              Error(os.str());
            }
          }
        }
      }

      /**
       *  **********************************************************************
       *  **********************************************************************
       *  @defgroup RESHAPE Matrix reshaping rutines
       *  **********************************************************************
       *  **********************************************************************
       * @{
       */

      /**
       *  @brief Removes one row from the matrix. The memory is not reallocated.
       */
      ThisType &
      RemoveRow(size_t i);

      /** @} */

      /**
       *  **********************************************************************
       *  **********************************************************************
       *  @defgroup ACCESS Access functions and operators
       *  **********************************************************************
       *  **********************************************************************
       * @{
       */

      /**
       *  @brief Gives access to a specified matrix row without range check
       *  @return Subvector object representing the row
       */
      inline const SubVector<_ElemT>
      operator []  (size_t i) const
      {
        assert(i < mMRows);
        return SubVector<_ElemT>(mpData + (i * mStride), Cols());
      }

      inline SubVector<_ElemT>
      operator []  (size_t i)
      {
        assert(i < mMRows);
        return SubVector<_ElemT>(mpData + (i * mStride), Cols());
      }

      /**
       *  @brief Gives access to a specified matrix row without range check
       *  @return pointer to the first field of the row
       */
      inline  _ElemT*
      pRowData(size_t i)
      {
        assert(i < mMRows);
        return mpData + i * mStride;
      }

      /**
       *  @brief Gives access to a specified matrix row without range check
       *  @return pointer to the first field of the row (const version)
       */
      inline const _ElemT*
      pRowData(size_t i) const
      {
        assert(i < mMRows);
        return mpData + i * mStride;
      }

      /**
       *  @brief Gives access to matrix elements (row, col)
       *  @return reference to the desired field
       */
      inline _ElemT&
		operator () (size_t r, size_t c)
      {
#ifdef PARANOID
        assert(r < mMRows && c < mMCols);
#endif
		return *(mpData + r * mStride + c);
	  }

      /**
       *  @brief Gives access to matrix elements (row, col)
       *  @return pointer to the desired field (const version)
       */
      inline const _ElemT
		operator () (size_t r, size_t c) const
      {
#ifdef PARANOID
        assert(r < mMRows && c < mMCols);
#endif
		return *(mpData + r * mStride + c);
	  }

      /**
       * @brief Returns a matrix sub-range
       * @param ro Row offset
       * @param r  Rows in range
       * @param co Column offset
       * @param c  Coluns in range
       * See @c SubMatrix class for details
       */
      SubMatrix<_ElemT>
      Range(const size_t    ro, const size_t    r,
            const size_t    co, const size_t    c)
      { return SubMatrix<_ElemT>(*this, ro, r, co, c); }

      const SubMatrix<_ElemT>
      Range(const size_t    ro, const size_t    r,
            const size_t    co, const size_t    c) const
      { return SubMatrix<_ElemT>(*this, ro, r, co, c); }
      /** @} */

      /**
       *  **********************************************************************
       *  **********************************************************************
       *  @defgroup MATH ROUTINES
       *  **********************************************************************
       *  **********************************************************************
       *  @{
       **/

      /**
       *  @brief Returns sum of all elements
       */
      _ElemT&
      Sum() const;

      ThisType &
      DotMul(const ThisType& a);

      ThisType &
      Scale(_ElemT alpha);

      ThisType &
      ScaleCols(const Vector<_ElemT> &scale); // Equivalent to (*this) = (*this) * diag(scale).

      ThisType &
      ScaleRows(const Vector<_ElemT> &scale); // Equivalent to (*this) = diag(scale) * (*this);

      /// Sum another matrix rMatrix with this matrix
      ThisType&
      Add(const Matrix<_ElemT>& rMatrix);

      /// Sum scaled matrix rMatrix with this matrix
      ThisType&
      AddScaled(_ElemT alpha, const Matrix<_ElemT>& rMatrix);

      /// Apply log to all items of the matrix
      ThisType&
      ApplyLog();

      /**
       * @brief Computes the determinant of this matrix
       * @return Returns the determinant of a matrix
       * @ingroup MATH
       *
       */
      _ElemT LogAbsDeterminant(_ElemT *DetSign=NULL);

      /**
       *  @brief Performs matrix inplace inversion
       */
      ThisType &
      Invert(_ElemT *LogDet=NULL, _ElemT *DetSign=NULL, bool inverse_needed=true);

      /**
       *  @brief Performs matrix inplace inversion in double precision, even if this object is not double precision.
       */
      ThisType &
      InvertDouble(_ElemT *LogDet=NULL, _ElemT *DetSign=NULL, bool inverse_needed=true){
        double LogDet_tmp, DetSign_tmp;
        Matrix<double> dmat(*this); dmat.Invert(&LogDet_tmp, &DetSign_tmp, inverse_needed); if(inverse_needed) (*this).Copy(dmat);
        if(LogDet) *LogDet = LogDet_tmp; if(DetSign) *DetSign = DetSign_tmp;
        return *this;
      }

      /**
       *  @brief Inplace matrix transposition. Applicable only to square matrices
       */
      ThisType &
      Transpose()
      {
        assert(Rows()==Cols());
        size_t M=Rows();
        for(size_t i=0;i<M;i++)
          for(size_t j=0;j<i;j++){
           _ElemT &a = (*this)(i,j), &b = (*this)(j,i);
		   std::swap(a,b);
        }
		return *this;
      }

      bool IsSymmetric(_ElemT cutoff = 1.0e-05) const;

      bool IsDiagonal(_ElemT cutoff = 1.0e-05) const;

      bool IsUnit(_ElemT cutoff = 1.0e-05) const;

      bool IsZero(_ElemT cutoff = 1.0e-05) const;

      _ElemT FrobeniusNorm() const; // sqrt of sum of square elements.

      _ElemT LargestAbsElem() const; // largest absolute value.

      friend _ElemT TNet::TraceOfProduct<_ElemT>(const Matrix<_ElemT> &A, const Matrix<_ElemT> &B); // tr(A B)
      friend _ElemT TNet::TraceOfProductT<_ElemT>(const Matrix<_ElemT> &A, const Matrix<_ElemT> &B); // tr(A B^T)==tr(A^T B)
      friend class SubMatrix<_ElemT>; // so it can get around const restrictions on the pointer to mpData.

      /** **********************************************************************
       *  **********************************************************************
       *  @defgroup BLAS_ROUTINES BLAS ROUTINES
       *  @ingroup MATH
       *  **********************************************************************
       *  **********************************************************************
       **/

      ThisType &
      BlasGer(const _ElemT alpha, const Vector<_ElemT>& rA, const Vector<_ElemT>& rB);

      ThisType &
	    Axpy(const _ElemT alpha, const Matrix<_ElemT> &rM, MatrixTrasposeType transA=NO_TRANS);

      ThisType &
      BlasGemm(const _ElemT alpha,
               const ThisType& rA, MatrixTrasposeType transA,
               const ThisType& rB, MatrixTrasposeType transB,
               const _ElemT beta = 0.0);

      /** @} */

      /** **********************************************************************
       *  **********************************************************************
       *  @defgroup IO Input/Output ROUTINES
       *  **********************************************************************
       *  **********************************************************************
       *  @{
       **/

      friend std::ostream &
      operator << <> (std::ostream & out, const ThisType & m);

      void PrintOut(char *file);
      void ReadIn(char *file);

      bool
      LoadHTK(const char* pFileName);

      /** @} */

    protected:
//      inline void swap4b(void *a);
//      inline void swap2b(void *a);

    protected:
      /// data memory area
      _ElemT*   mpData;

      /// these atributes store the real matrix size as it is stored in memory
      /// including memalignment
      size_t    mMCols;       ///< Number of columns
      size_t    mMRows;       ///< Number of rows
      size_t    mStride;      ///< true number of columns for the internal matrix.
                              ///< This number may differ from M_cols as memory
                              ///< alignment might be used

#ifdef STK_MEMALIGN_MANUAL
      /// data to be freed (in case of manual memalignment use, see Common.h)
      _ElemT*   mpFreeData;
#endif
    }; // class Matrix

    template<>  Matrix<float> &  Matrix<float>::Invert(float *LogDet, float *DetSign, bool inverse_needed); // state that we will implement separately for float and double.
    template<>  Matrix<double> &  Matrix<double>::Invert(double *LogDet, double *DetSign, bool inverse_needed);

  /** **************************************************************************
   ** **************************************************************************
   *  @brief Sub-matrix representation
   *
   *  This class provides a way to work with matrix cutouts in STK.
   *
   *
   */
  template<typename _ElemT>
    class SubMatrix : public Matrix<_ElemT>
    {
    typedef SubMatrix<_ElemT>    ThisType;

    public:
      /// Constructor
      SubMatrix(const Matrix<_ElemT>& rT, // Input matrix cannot be const because SubMatrix can change its contents.
                const size_t    ro,
                const size_t    r,
                const size_t    co,
                const size_t    c);

      /// The destructor
      ~SubMatrix<_ElemT>()
      {
#ifndef STK_MEMALIGN_MANUAL
        Matrix<_ElemT>::mpData = NULL;
#else
        Matrix<_ElemT>::mpFreeData = NULL;
#endif
      }

      /// Assign operator
      ThisType& operator=(const ThisType& rSrc)
      {
        //std::cout << "[PERFORMing operator= SubMatrix&^2]" << std::flush;
        this->mpData = rSrc.mpData;
        this->mMCols = rSrc.mMCols;
        this->mMRows = rSrc.mMRows;
        this->mStride = rSrc.mStride;
        this->mpFreeData = rSrc.mpFreeData;
        return *this;
      }

      /// Initializes matrix (if not done by constructor)
      ThisType &
      Init(const size_t r,
           const size_t c, bool clear=true)
      { Error("Submatrix cannot do Init"); return *this; }

      /**
       * @brief Dealocates the matrix from memory and resets the dimensions to (0, 0)
       */
      void
      Destroy()
      { Error("Submatrix cannot do Destroy"); }

    };

  //Create useful shortcuts
  typedef Matrix<BaseFloat> BfMatrix;
  typedef SubMatrix<BaseFloat> BfSubMatrix;

  /**
   * Function for summing matrices of different types
   */
  template<typename _ElemT, typename _ElemU>
  void Add(Matrix<_ElemT>& rDst,  const Matrix<_ElemU>& rSrc) {
    assert(rDst.Cols() == rSrc.Cols());
    assert(rDst.Rows() == rSrc.Rows());

    for(size_t i=0; i<rDst.Rows(); i++) {
      const _ElemU* p_src = rSrc.pRowData(i);
      _ElemT* p_dst = rDst.pRowData(i);
      for(size_t j=0; j<rDst.Cols(); j++) {
        *p_dst++ += (_ElemT)*p_src++;
      }
    }
  }

  /**
   * Function for summing matrices of different types
   */
  template<typename _ElemT, typename _ElemU>
  void AddScaled(Matrix<_ElemT>& rDst, const Matrix<_ElemU>& rSrc, _ElemT scale) {
    assert(rDst.Cols() == rSrc.Cols());
    assert(rDst.Rows() == rSrc.Rows());

    Vector<_ElemT> tmp(rDst[0]);

    for(size_t i=0; i<rDst.Rows(); i++) {
      tmp.Copy(rSrc[i]);
      rDst[i].BlasAxpy(scale, tmp);

      /*
      const _ElemU* p_src = rSrc.pRowData(i);
      _ElemT* p_dst = rDst.pRowData(i);
      for(size_t j=0; j<rDst.Cols(); j++) {
        *p_dst++ += (_ElemT)(*p_src++) * scale;
      }
      */
    }
  }

} // namespace STK

//*****************************************************************************
//*****************************************************************************
// we need to include the implementation
#include "Matrix.tcc"
//*****************************************************************************
//*****************************************************************************

/******************************************************************************
 ******************************************************************************
 * The following section contains specialized template definitions
 * whose implementation is in Matrix.cc
 */

//#ifndef TNet_Matrix_h
#endif

max_stars_repo_name: troylee/nnet-asr

max_issues_repo_name: troylee/nnet-asr

max_forks_repo_name: troylee/nnet-asr


text:
/**
    General definitions

    @author Álvaro Barbero Jiménez
    @author Suvrit Sra
*/
#ifndef _GENERAL_H
#define _GENERAL_H

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <float.h>
#include <limits.h>

/* Mex and LAPACK includes */
#ifdef NOMATLAB
//#undef lapack_int
//#define lapack_int              long
//#include <lapacke.h>
//#undef MKL_INT
//#define MKL_INT              long
#include <mkl.h>
inline double mxGetInf() { return INFINITY; }
#else
#include "mex.h"
#include "lapack.h"
#include "matrix.h"
#define lapack_int  ptrdiff_t
#endif

/* Uncomment to print debug messages to a debug file */
//#define DEBUG
/* Choose here the name of the debug file */
#ifdef DEBUG
    static FILE* DEBUG_FILE = fopen("debug.tmp","w");
#else
    static FILE* DEBUG_FILE = NULL;
#endif
#define DEBUG_N 10 /* Maximum vector length to print in debug messages */

/* Uncomment to produce timing messages (in some of the algorithms) */
//#define TIMING

/* Includes for parallel computation */
#ifdef _OPENMP
    #include <omp.h>
#else
    #define omp_get_thread_num() 0
    #define omp_get_max_threads() 1
    #define omp_set_num_threads(nThreads) /**/;
#endif

/* Indexes of info structure of solvers */
#define N_INFO 3
#define INFO_ITERS 0
#define INFO_GAP 1
#define INFO_RC 2

/* Comparison tolerance */
#define EPSILON 1e-10
#define IS_ZERO(x) (x < EPSILON & x > -EPSILON)
#define IS_POSITIVE(x) (x > EPSILON)
#define IS_NEGATIVE(x) (x < -EPSILON)

/* Return Codes */
#define RC_OK 0 // Solution found at the specified error level
#define RC_ITERS 1 // Maximum number of iterations reaches, result might be suboptimal
#define RC_STUCK 2 // Algorithm stuck, impossible to improve the objetive function further, result might be suboptimal
#define RC_ERROR 3 // Fatal error during the algorithm, value of the solution is undefined.

#endif

ext: h

max_stars_repo_name: hijizhou/NeuronDemix

max_issues_repo_name: hijizhou/NeuronDemix

max_forks_repo_name: hijizhou/NeuronDemix


text:
/*
  NAME:
     splitnmergegauss
  PURPOSE:
     split one gaussian and merge two other gaussians
  CALLING SEQUENCE:
     splitnmergegauss(struct gaussian * gaussians,int K, gsl_matrix * qij,
     int j, int k, int l)
  INPUT:
     gaussians   - model gaussians
     K           - number of gaussians
     qij         - matrix of log(posterior likelihoods)
     j,k         - gaussians that need to be merged
     l           - gaussian that needs to be split
  OUTPUT:
     updated gaussians
  REVISION HISTORY:
     2008-09-21 - Written Bovy
*/
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include "proj_gauss_mixtures.h"

void splitnmergegauss(struct gaussian * gaussians,int K,
		      gsl_matrix * qij, int j, int k, int l){
  //get the gaussians to be split 'n' merged
  int d = (gaussians->VV)->size1;//dim of mm
  //int partial_indx[]= {-1,-1,-1};/* dummy argument for logsum */
  //bool * dummy_allfixed = (bool *) calloc(K,sizeof(bool));
  //j,k,l gaussians
  struct gaussian gaussianj, gaussiank, gaussianl;
  gaussianj.mm = gsl_vector_alloc(d);
  gaussianj.VV = gsl_matrix_alloc(d,d);
  gaussiank.mm = gsl_vector_alloc(d);
  gaussiank.VV = gsl_matrix_alloc(d,d);
  gaussianl.mm = gsl_vector_alloc(d);
  gaussianl.VV = gsl_matrix_alloc(d,d);

  gsl_matrix * unitm = gsl_matrix_alloc(d,d);
  gsl_matrix_set_identity(unitm);
  gsl_vector * eps = gsl_vector_alloc(d);
  double qjj,qjk,detVVjl;
  int kk;
  for (kk = 0; kk != K; ++kk){
    if (kk == j){
      gaussianj.alpha = gaussians->alpha;
      gsl_vector_memcpy(gaussianj.mm,gaussians->mm);
      gsl_matrix_memcpy(gaussianj.VV,gaussians->VV);
      qjj = exp(logsum(qij,j,false));//,dummy_allfixed));
    }
    if (kk == k){
      gaussiank.alpha = gaussians->alpha;
      gsl_vector_memcpy(gaussiank.mm,gaussians->mm);
      gsl_matrix_memcpy(gaussiank.VV,gaussians->VV);
      qjk = exp(logsum(qij,k,false));//,dummy_allfixed));
    }
    if (kk == l){
      gaussianl.alpha = gaussians->alpha;
      gsl_vector_memcpy(gaussianl.mm,gaussians->mm);
      gsl_matrix_memcpy(gaussianl.VV,gaussians->VV);
    }
    ++gaussians;
  }
  gaussians -= K;

  //merge j & k
  gaussianj.alpha += gaussiank.alpha;
  if (qjk == 0. && qjj == 0){
    gsl_vector_add(gaussianj.mm,gaussiank.mm);
    gsl_vector_scale(gaussianj.mm,0.5);
    gsl_matrix_add(gaussianj.VV,gaussiank.VV);
    gsl_matrix_scale(gaussianj.VV,0.5);
  }
  else{
    gsl_vector_scale(gaussianj.mm,qjj/(qjj+qjk));
    gsl_vector_scale(gaussiank.mm,qjk/(qjj+qjk));
    gsl_vector_add(gaussianj.mm,gaussiank.mm);
    gsl_matrix_scale(gaussianj.VV,qjj/(qjj+qjk));
    gsl_matrix_scale(gaussiank.VV,qjk/(qjj+qjk));
    gsl_matrix_add(gaussianj.VV,gaussiank.VV);
  }

  //split l
  gaussianl.alpha /= 2.;
  gaussiank.alpha = gaussianl.alpha;
  detVVjl = bovy_det(gaussianl.VV);
  detVVjl= pow(detVVjl,1./d);
  gsl_matrix_scale(unitm,detVVjl);
  gsl_matrix_memcpy(gaussiank.VV,unitm);
  gsl_matrix_memcpy(gaussianl.VV,unitm);
  gsl_vector_memcpy(gaussiank.mm,gaussianl.mm);
  bovy_randvec(eps,d,sqrt(detVVjl));
  gsl_vector_add(gaussiank.mm,eps);
  bovy_randvec(eps,d,sqrt(detVVjl));
  gsl_vector_add(gaussianl.mm,eps);

  //copy everything back into the right gaussians
  for (kk = 0; kk != K; ++kk){
    if (kk == j){
      gaussians->alpha = gaussianj.alpha;
      gsl_vector_memcpy(gaussians->mm,gaussianj.mm);
      gsl_matrix_memcpy(gaussians->VV,gaussianj.VV);
    }
    if (kk == k){
      gaussians->alpha = gaussiank.alpha;
      gsl_vector_memcpy(gaussians->mm,gaussiank.mm);
      gsl_matrix_memcpy(gaussians->VV,gaussiank.VV);
    }
    if (kk == l){
      gaussians->alpha = gaussianl.alpha;
      gsl_vector_memcpy(gaussians->mm,gaussianl.mm);
      gsl_matrix_memcpy(gaussians->VV,gaussianl.VV);
    }
    ++gaussians;
  }
  gaussians -= K;

  //cleanup
  gsl_matrix_free(unitm);
  gsl_vector_free(eps);
  //free(dummy_allfixed);

  return ;
}

ext: c

max_stars_repo_name: surbut/mashr

max_issues_repo_name: surbut/mashr

max_forks_repo_name: surbut/mashr


text:
#ifndef _PURSUIT_SHIP_H_
#define _PURSUIT_SHIP_H_

//-----------------------------------------------------------------------------

#include <boost/shared_ptr.hpp>

#include <Ravelin/Pose3d.h>
#include <Ravelin/SpatialRBInertiad.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit_nlin.h>

#include <ompl/control/SpaceInformation.h>
#include <ompl/base/goals/GoalState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/planners/rrt/RRT.h>
#include <ompl/control/SimpleSetup.h>
#include <ompl/config.h>
#include <ompl/control/SimpleDirectedControlSampler.h>
#include <ompl/control/ODESolver.h>

#include "experiment.h"
#include "aabb.h"
#include "control_space.h"

//-----------------------------------------------------------------------------

class space_c;
typedef boost::shared_ptr<space_c> space_p;
class ship_c;
typedef boost::shared_ptr<ship_c> ship_p;

//-----------------------------------------------------------------------------
class ship_c {
public:

  //---------------------------------------------------------------------------
  // Members
  //---------------------------------------------------------------------------

  Ravelin::Pose3d _pose;
  Ravelin::SVelocityd _velocity;

  // the inertial mass matrix of the ship
  Ravelin::SpatialRBInertiad _inertial;

  space_p _space;
  //----------

  // the set of possible player types
  enum role_e {
    NONE = 0,
    PREY,
    PREDATOR
  };

  role_e role;

  ship_p adversary;

  double time;
  double dtime;

  double integration_step;
  double controller_step;

  // the ship's bounding box in the ship's frame of reference
  aabb_c ship_frame_bb;

   // whether or not a capture event has occurred
  bool capture;
  bool stopped;

  //---------------------------------------------------------------------------
  // Constructors
  //---------------------------------------------------------------------------
  ship_c( role_e role );
  ship_c( ompl::base::StateSpace *_statespace );

  //---------------------------------------------------------------------------
  // Destructor
  //---------------------------------------------------------------------------
  virtual ~ship_c( void );

  //---------------------------------------------------------------------------
  // Base Class Definitions
  //---------------------------------------------------------------------------
  virtual bool open( void ) { return false; }
  virtual bool read( std::vector<double>& q ) { return false; }
  virtual bool write( const std::vector<double>& u ) { return false; }
  virtual void reset( void ) { }

  virtual bool read_ke( double& ke ) { return false; }

public:
  static unsigned long compute_prey_command_seed( double time, double dtime );
  static double compute_predator_time_randval( double time, double dtime );

  // computes commands (forces) if the ship is prey
  static void compute_prey_command( const std::vector<double>& pred_state, const std::vector<double>& prey_state, std::vector<double>& prey_u, const double& time, const double& dtime, space_c* space );

  // normalizes the quaternion components of the state
  static void renormalize_state_quat(std::vector<double>& q);

  // compute the bounding box for the ship given a state
  aabb_c aabb( const std::vector<double>& q );

  // query whether a bounding box intersects another bounding box
  //bool intersects_any_obstacle( const aabb_c& mybb, aabb_c& obstacle );

  // query whether the ship intersects the world bounding box
  bool intersects_world_bounds( const aabb_c& mybb );

  // compute any force(field) that the boundary contributes to repel collision
  static Ravelin::Vector3d boundary_force( space_c* space, const Ravelin::Vector3d& pos, const Ravelin::Vector3d& vel );

  static Ravelin::SForced drag_force( Ravelin::SVelocityd& v, boost::shared_ptr<Ravelin::Pose3d> P_CoM );

  // computes a repulsive force for a given distance
  static double repulsive_force( double repulsion_factor, double dist );
  static double flee_force( double repulsion_factor, double dist );

  // - Feedback Control -
  // compute the desired state based on the current command and current state
  void compute_desired_state( const std::vector<double>& u, const std::vector<double>& x_current, std::vector<double>& x_desired );
 // computes the feedback command based on error in the current state
  void compute_feedback( const std::vector<double>& x_current, std::vector<double>& x_desired, const std::vector<double>& u_current, std::vector<double>& u_feedback );

  // - Dynamics -
  // compute the inverse dynamics for the ship
  static void inv_dyn( space_c* space,  const std::vector<double>& q, const std::vector<double>& qdot_des, std::vector<double>& u, const Ravelin::SpatialRBInertiad& inertial );

  // ordinary differential equations for the ship
  static void ode( space_c* space, const std::vector<double>& q, const std::vector<double>& u, std::vector<double>& dq, const Ravelin::SpatialRBInertiad& inertial );

  // - OMPL --
  // plans using the ompl rrt planner
  //bool plan_rrt( const std::vector<double>& pred_q, const std::vector<double>& prey_q, std::vector<double>& u );

  bool compute_predator_plan( const std::vector<double>& pred_q, const std::vector<double>& prey_q, std::vector< std::vector<double> >& us, std::vector<double>& durations, unsigned& control_count );

 // the reference to the statespace the planner has generated
  ompl::base::StateSpace *_statespace;

  // query whether or not the state generated by the planner is valid
  bool is_state_valid(const ompl::control::SpaceInformation *si, const ompl::base::State *state);

  // allocates the predator/prey control sampler for the planner
  ompl::control::DirectedControlSamplerPtr allocate_pp_control_sampler( const ompl::control::SpaceInformation* si );

  // ode.  called by the eular_integrator_c in integrator.h
  void operator()( const ompl::base::State* state, const ompl::control::Control* control, std::vector<double>& dstate, const Ravelin::SpatialRBInertiad& inertial ) const;

  // post integration update method
  void update( ompl::base::State* state, const std::vector<double>& dstate ) const;

  static double compute_distance( const std::vector<double>& pred_state, const std::vector<double>& prey_state );

  private:
    void get_predator_control(const std::vector<double>& last_pred_q, const std::vector<double>& new_prey_q, const Ravelin::SpatialRBInertiad& predJ, space_c* space, double DT, std::vector<double>& u);
    void integrate_prey(const std::vector<double>& prey_q, const std::vector<double>& pred_q, const Ravelin::SpatialRBInertiad& Jprey, double time, space_c* space, double DT, std::vector<double>& qnew);
    void integrate_predator(const std::vector<double>& q, const std::vector<double>& u, const Ravelin::SpatialRBInertiad& J, space_c* space, double DT, std::vector<double>& qnew);
};

#endif // _PURSUIT_SHIP_H_

max_stars_repo_name: PositronicsLab/tcs

max_issues_repo_name: PositronicsLab/tcs

max_forks_repo_name: PositronicsLab/tcs


text:
/* interpolation/akima.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include "integ_eval.h"
#include <gsl/gsl_interp.h>

typedef struct
{
  double * b;
  double * c;
  double * d;
  double * _m;
} akima_state_t;

/* common creation */
static void *
akima_alloc (size_t size)
{
  akima_state_t *state = (akima_state_t *) malloc (sizeof (akima_state_t));

  if (state == NULL)
    {
      GSL_ERROR_NULL("failed to allocate space for state", GSL_ENOMEM);
    }

  state->b = (double *) malloc (size * sizeof (double));

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

  state->c = (double *) malloc (size * sizeof (double));

  if (state->c == NULL)
    {
      free (state->b);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for c", GSL_ENOMEM);
    }

  state->d = (double *) malloc (size * sizeof (double));

  if (state->d == NULL)
    {
      free (state->c);
      free (state->b);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for d", GSL_ENOMEM);
    }

  state->_m = (double *) malloc ((size + 4) * sizeof (double));

  if (state->_m == NULL)
    {
      free (state->d);
      free (state->c);
      free (state->b);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for _m", GSL_ENOMEM);
    }

  return state;
}

/* common calculation */
static void
akima_calc (const double x_array[], double b[],  double c[],  double d[], size_t size, double m[])
{
  size_t i;

  for (i = 0; i < (size - 1); i++)
    {
      const double NE = fabs (m[i + 1] - m[i]) + fabs (m[i - 1] - m[i - 2]);
      if (NE == 0.0)
        {
          b[i] = m[i];
          c[i] = 0.0;
          d[i] = 0.0;
        }
      else
        {
          const double h_i = x_array[i + 1] - x_array[i];
          const double NE_next = fabs (m[i + 2] - m[i + 1]) + fabs (m[i] - m[i - 1]);
          const double alpha_i = fabs (m[i - 1] - m[i - 2]) / NE;
          double alpha_ip1;
          double tL_ip1;
          if (NE_next == 0.0)
            {
              tL_ip1 = m[i];
            }
          else
            {
              alpha_ip1 = fabs (m[i] - m[i - 1]) / NE_next;
              tL_ip1 = (1.0 - alpha_ip1) * m[i] + alpha_ip1 * m[i + 1];
            }
          b[i] = (1.0 - alpha_i) * m[i - 1] + alpha_i * m[i];
          c[i] = (3.0 * m[i] - 2.0 * b[i] - tL_ip1) / h_i;
          d[i] = (b[i] + tL_ip1 - 2.0 * m[i]) / (h_i * h_i);
        }
    }
}

static int
akima_init (void * vstate, const double x_array[], const double y_array[],
            size_t size)
{
  akima_state_t *state = (akima_state_t *) vstate;

  double * m = state->_m + 2; /* offset so we can address the -1,-2
                                 components */

  size_t i;
  for (i = 0; i <= size - 2; i++)
    {
      m[i] = (y_array[i + 1] - y_array[i]) / (x_array[i + 1] - x_array[i]);
    }

  /* non-periodic boundary conditions */
  m[-2] = 3.0 * m[0] - 2.0 * m[1];
  m[-1] = 2.0 * m[0] - m[1];
  m[size - 1] = 2.0 * m[size - 2] - m[size - 3];
  m[size] = 3.0 * m[size - 2] - 2.0 * m[size - 3];

  akima_calc (x_array, state->b, state->c, state->d, size, m);

  return GSL_SUCCESS;
}

static int
akima_init_periodic (void * vstate,
                     const double x_array[],
                     const double y_array[],
                     size_t size)
{
  akima_state_t *state = (akima_state_t *) vstate;

  double * m = state->_m + 2; /* offset so we can address the -1,-2
                                 components */

  size_t i;
  for (i = 0; i <= size - 2; i++)
    {
      m[i] = (y_array[i + 1] - y_array[i]) / (x_array[i + 1] - x_array[i]);
    }

  /* periodic boundary conditions */
  m[-2] = m[size - 1 - 2];
  m[-1] = m[size - 1 - 1];
  m[size - 1] = m[0];
  m[size] = m[1];

  akima_calc (x_array, state->b, state->c, state->d, size, m);

  return GSL_SUCCESS;
}

static void
akima_free (void * vstate)
{
  akima_state_t *state = (akima_state_t *) vstate;

  free (state->b);
  free (state->c);
  free (state->d);
  free (state->_m);
  free (state);
}

static
int
akima_eval (const void * vstate,
            const double x_array[], const double y_array[], size_t size,
            double x,
            gsl_interp_accel * a,
            double *y)
{
  const akima_state_t *state = (const akima_state_t *) vstate;

  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  {
    const double x_lo = x_array[index];
    const double delx = x - x_lo;
    const double b = state->b[index];
    const double c = state->c[index];
    const double d = state->d[index];
    *y = y_array[index] + delx * (b + delx * (c + d * delx));
    return GSL_SUCCESS;
  }
}

static int
akima_eval_deriv (const void * vstate,
                  const double x_array[], const double y_array[], size_t size,
                  double x,
                  gsl_interp_accel * a,
                  double *dydx)
{
  const akima_state_t *state = (const akima_state_t *) vstate;

  size_t index;

  DISCARD_POINTER(y_array); /* prevent warning about unused parameter */

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  {
    double x_lo = x_array[index];
    double delx = x - x_lo;
    double b = state->b[index];
    double c = state->c[index];
    double d = state->d[index];
    *dydx = b + delx * (2.0 * c + 3.0 * d * delx);
    return GSL_SUCCESS;
  }
}

static
int
akima_eval_deriv2 (const void * vstate,
                   const double x_array[], const double y_array[], size_t size,
                   double x,
                   gsl_interp_accel * a,
                   double *y_pp)
{
  const akima_state_t *state = (const akima_state_t *) vstate;

  size_t index;

  DISCARD_POINTER(y_array); /* prevent warning about unused parameter */

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  {
    const double x_lo = x_array[index];
    const double delx = x - x_lo;
    const double c = state->c[index];
    const double d = state->d[index];
    *y_pp = 2.0 * c + 6.0 * d * delx;
    return GSL_SUCCESS;
  }
}

static
int
akima_eval_integ (const void * vstate,
                  const double x_array[], const double y_array[], size_t size,
                  gsl_interp_accel * acc,
                  double a, double b,
                  double * result)
{
  const akima_state_t *state = (const akima_state_t *) vstate;

  size_t i, index_a, index_b;

  if (acc != 0)
    {
      index_a = gsl_interp_accel_find (acc, x_array, size, a);
      index_b = gsl_interp_accel_find (acc, x_array, size, b);
    }
  else
    {
      index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);
      index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);
    }

  *result = 0.0;

  /* interior intervals */

  for(i=index_a; i<=index_b; i++) {
    const double x_hi = x_array[i + 1];
    const double x_lo = x_array[i];
    const double y_lo = y_array[i];
    const double dx = x_hi - x_lo;
    if(dx != 0.0) {

      if (i == index_a || i == index_b)
        {
          double x1 = (i == index_a) ? a : x_lo;
          double x2 = (i == index_b) ? b : x_hi;
          *result += integ_eval (y_lo, state->b[i], state->c[i], state->d[i],
                                 x_lo, x1, x2);
        }
      else
        {
          *result += dx * (y_lo
                           + dx*(0.5*state->b[i]
                                 + dx*(state->c[i]/3.0
                                       + 0.25*state->d[i]*dx)));
        }
    }
    else {
      *result = 0.0;
      return GSL_EINVAL;
    }
  }

  return GSL_SUCCESS;
}

static const gsl_interp_type akima_type =
{
  "akima",
  5,
  &akima_alloc,
  &akima_init,
  &akima_eval,
  &akima_eval_deriv,
  &akima_eval_deriv2,
  &akima_eval_integ,
  &akima_free
};

const gsl_interp_type * gsl_interp_akima = &akima_type;

static const gsl_interp_type akima_periodic_type =
{
  "akima-periodic",
  5,
  &akima_alloc,
  &akima_init_periodic,
  &akima_eval,
  &akima_eval_deriv,
  &akima_eval_deriv2,
  &akima_eval_integ,
  &akima_free
};

const gsl_interp_type * gsl_interp_akima_periodic = &akima_periodic_type;

max_stars_repo_name: ielomariala/Hex-Game

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
///
/// @file This file contains OpenMP and BLAS thread count configuration
/// interface.
///
/// @author Mirko Myllykoski (mirkom@cs.umu.se), Umeå University
///
/// @internal LICENSE
///
/// Copyright (c) 2019-2020, Umeå Universitet
///
/// Redistribution and use in source and binary forms, with or without
/// modification, are permitted provided that the following conditions are met:
///
/// 1. Redistributions of source code must retain the above copyright notice,
///    this list of conditions and the following disclaimer.
///
/// 2. Redistributions in binary form must reproduce the above copyright notice,
///    this list of conditions and the following disclaimer in the documentation
///    and/or other materials provided with the distribution.
///
/// 3. Neither the name of the copyright holder nor the names of its
///    contributors may be used to endorse or promote products derived from this
///    software without specific prior written permission.
///
/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
/// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
/// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
/// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
/// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
/// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
/// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
/// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
/// POSSIBILITY OF SUCH DAMAGE.
///

#include <starneig_test_config.h>
#include <starneig/configuration.h>
#include "threads.h"
#include "parse.h"
#include "omp.h"
#include <hwloc.h>
#ifdef MKL_SET_NUM_THREADS_LOCAL_FOUND
#include <mkl.h>
#endif
#if defined(OPENBLAS_SET_NUM_THREADS_FOUND) || \
defined(GOTO_SET_NUM_THREADS_FOUND)
#include <cblas.h>
#endif

static struct {
    int worker_threads;
    int blas_threads;
    int lapack_threads;
    int scalapack_threads;
} status = {
    .worker_threads = 1,
    .blas_threads = 1,
    .lapack_threads = 1,
    .scalapack_threads = 1
};

static int get_core_count()
{
    hwloc_topology_t topology;
    hwloc_topology_init(&topology);
    hwloc_topology_load(topology);

    hwloc_cpuset_t res = hwloc_bitmap_alloc();

    hwloc_cpuset_t mask = hwloc_bitmap_alloc();
    hwloc_get_cpubind(topology, mask, HWLOC_CPUBIND_THREAD);

    int depth_cores = hwloc_get_type_depth(topology, HWLOC_OBJ_CORE);
    int num_cores = hwloc_get_nbobjs_by_depth(topology, depth_cores);

    int num_hwloc_cpus = 0;

    // iterate over all COREs
    for (int i = 0; i < num_cores; i++) {
        hwloc_obj_t core = hwloc_get_obj_by_depth(topology, depth_cores, i);

        // if the CORE has PUs inside it, ...
        if (core->first_child && core->first_child->type == HWLOC_OBJ_PU) {
            // iterate over them
            hwloc_obj_t pu = core->first_child;
            while (pu) {
                // if the PU is in the binding mask, ...
                hwloc_bitmap_and(res, mask, pu->cpuset);
                if (!hwloc_bitmap_iszero(res)) {
                    // count the PU as a CORE
                    num_hwloc_cpus++;
                    break;
                }
                pu = pu->next_sibling;
            }
        }
        else {
            // if the CORE is in the binding mask, ...
            hwloc_bitmap_and(res, mask, core->cpuset);
            if (!hwloc_bitmap_iszero(res)) {
                // count the CORE
                num_hwloc_cpus++;
            }
        }
    }

    hwloc_bitmap_free(mask);
    hwloc_bitmap_free(res);
    hwloc_topology_destroy(topology);

    return num_hwloc_cpus;
}

static void set_blas_threads(int threads)
{
#if defined(MKL_SET_NUM_THREADS_LOCAL_FOUND)
    mkl_set_num_threads_local(threads);
#elif defined(OPENBLAS_SET_NUM_THREADS_FOUND)
    openblas_set_num_threads(threads);
#elif defined(GOTO_SET_NUM_THREADS_FOUND)
    goto_set_num_threads(threads);
#endif
}

void thread_print_usage(int argc, char * const *argv)
{
    printf(
        "  --test-workers [(num),default] -- Test program StarPU worker count\n"
        "  --blas-threads [(num),default] -- Test program BLAS thread count\n"
        "  --lapack-threads [(num),default] -- LAPACK solver thread count\n"
        "  --scalapack-threads [(num),default] -- ScaLAPACK solver thread "
        "count\n"
    );
}

void thread_print_args(int argc, char * const *argv)
{
    print_multiarg("--test-workers", argc, argv, "default", NULL);
    print_multiarg("--blas-threads", argc, argv, "default", NULL);
    print_multiarg("--lapack-threads", argc, argv, "default", NULL);
    print_multiarg("--scalapack-threads", argc, argv, "default", NULL);
}

int thread_check_args(int argc, char * const *argv, int *argr)
{
    struct multiarg_t worker_threads =
        read_multiarg("--test-workers", argc, argv, argr, "default", NULL);

    if (worker_threads.type == MULTIARG_INVALID ||
    (worker_threads.type == MULTIARG_INT && worker_threads.int_value < 1)) {
        fprintf(stderr, "Invalid number of StarPU worker threads.\n");
        return 1;
    }

    struct multiarg_t blas_threads =
        read_multiarg("--blas-threads", argc, argv, argr, "default", NULL);

    if (blas_threads.type == MULTIARG_INVALID ||
    (blas_threads.type == MULTIARG_INT && blas_threads.int_value < 1)) {
        fprintf(stderr, "Invalid number of BLAS threads.\n");
        return 1;
    }

    struct multiarg_t lapack_threads =
        read_multiarg("--lapack-threads", argc, argv, argr, "default", NULL);

    if (lapack_threads.type == MULTIARG_INVALID ||
    (lapack_threads.type == MULTIARG_INT && lapack_threads.int_value < 1)) {
        fprintf(stderr, "Invalid number of LAPACK threads.\n");
        return 1;
    }

    struct multiarg_t scalapack_threads =
        read_multiarg("--scalapack-threads", argc, argv, argr, "default", NULL);

    if (scalapack_threads.type == MULTIARG_INVALID ||
    (scalapack_threads.type == MULTIARG_INT &&
    scalapack_threads.int_value < 1)) {
        fprintf(stderr, "Invalid number of ScaLAPACK threads.\n");
        return 1;
    }

    return 0;
}

void threads_init(int argc, char * const *argv)
{
    struct multiarg_t worker_threads =
        read_multiarg("--test-workers", argc, argv, NULL, "default", NULL);
    if (worker_threads.type == MULTIARG_INT)
        status.worker_threads = worker_threads.int_value;
    else
        status.worker_threads = get_core_count();
    printf(
        "THREADS: Using %d StarPU worker threads during initialization and "
        "validation.\n", status.worker_threads);

    struct multiarg_t blas_threads =
        read_multiarg("--blas-threads", argc, argv, NULL, "default", NULL);
    if (blas_threads.type == MULTIARG_INT)
        status.blas_threads = blas_threads.int_value;
    else
        status.blas_threads = get_core_count();
    printf(
        "THREADS: Using %d BLAS threads during initialization and "
        "validation.\n", status.blas_threads);

    struct multiarg_t lapack_threads =
        read_multiarg("--lapack-threads", argc, argv, NULL, "default", NULL);
    if (lapack_threads.type == MULTIARG_INT)
        status.lapack_threads = lapack_threads.int_value;
    else
        status.lapack_threads = get_core_count();
    printf(
        "THREADS: Using %d BLAS threads in LAPACK solvers.\n",
        status.lapack_threads);

    struct multiarg_t scalapack_threads =
        read_multiarg("--scalapack-threads", argc, argv, NULL, "default", NULL);
    if (scalapack_threads.type == MULTIARG_INT)
        status.scalapack_threads = scalapack_threads.int_value;
    else
        status.scalapack_threads = 1;
    printf(
        "THREADS: Using %d BLAS threads in ScaLAPACK solvers.\n",
        status.scalapack_threads);

    threads_set_mode(THREADS_MODE_DEFAULT);
}

void threads_set_mode(thread_mode_t mode)
{
    switch (mode) {
        case THREADS_MODE_BLAS:
            set_blas_threads(status.blas_threads);
            break;
        case THREADS_MODE_LAPACK:
            set_blas_threads(status.lapack_threads);
            break;
        case THREADS_MODE_SCALAPACK:
            set_blas_threads(status.scalapack_threads);
            break;
        default:
            set_blas_threads(1);
    }
}

int threads_get_workers()
{
    return status.worker_threads;
}

starneig_flag_t threads_get_fast_dm()
{
    if (1 < threads_get_workers())
        return STARNEIG_FAST_DM;
    return STARNEIG_HINT_DM;
}

max_stars_repo_name: NLAFET/StarNEig

max_issues_repo_name: NLAFET/StarNEig

max_forks_repo_name: NLAFET/StarNEig


text:
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_matrix.h>

#include "../csm_all.h"
#include "gpm.h"

#include <egsl/egsl_macros.h>

void sm_gpm(struct sm_params*params, struct sm_result*res) {
	res->valid = 0;
	/* Check for well-formedness of the input data */
	if(!ld_valid_fields(params->laser_ref) ||
	   !ld_valid_fields(params->laser_sens)) {
		return;
	}

	LDP laser_ref  = params->laser_ref;
	LDP laser_sens = params->laser_sens;

	/* We need to compute cartesian points */
	ld_compute_cartesian(laser_ref);
	/* ... and orientation */
	ld_simple_clustering(laser_ref, params->clustering_threshold);
	ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
	/* ... for both scans. */
	ld_compute_cartesian(laser_sens);
	ld_simple_clustering(laser_sens, params->clustering_threshold);
	ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);

	/* Create an histogram whose bin is large `theta_bin_size` */
	double theta_bin_size = deg2rad(params->gpm_theta_bin_size_deg);
	double hist_min = -M_PI-theta_bin_size; /* be robust */
	double hist_max = +M_PI+theta_bin_size;
	size_t nbins = (size_t) ceil(  (hist_max-hist_min) / theta_bin_size);
	gsl_histogram*hist = gsl_histogram_alloc(nbins);
	gsl_histogram_set_ranges_uniform(hist, hist_min, hist_max);

	/* Fill the histogram with samples */
	double u[3]; copy_d(params->first_guess, 3, u);
	sm_debug("gpm 1/2: old u = : %s \n", friendly_pose(u) );

	int interval = params->gpm_interval;

	int num_correspondences_theta=-1;

	ght_find_theta_range(laser_ref, laser_sens,
		u, params->max_linear_correction,
		params->max_angular_correction_deg, interval, hist, &num_correspondences_theta);

	if(num_correspondences_theta < laser_ref->nrays) {
		sm_error("sm_gpm(): I found only %d correspondences in the first pass of GPM. I consider it a failure.\n",
			num_correspondences_theta);
		return;
	}

	/* Find the bin with most samples */
	size_t max_bin = gsl_histogram_max_bin(hist);

	/* Around that value will be the range admissible for theta */
	double min_range, max_range;
	gsl_histogram_get_range(hist,max_bin,&min_range,&max_range);

	/* Extend the range of the search */
	double extend_range = deg2rad(params->gpm_extend_range_deg);
	min_range += -extend_range;
	max_range += +extend_range;

	/*	if(jf()) fprintf(jf(), "iteration 0\n");
	journal_pose("x_old", u);*/

	/*	if(jf()) fprintf(jf(), "iteration 1\n");
	journal_pose("x_old", u);*/

	/* Now repeat the samples generation with a smaller domain */
	u[2] = 0.5 * (max_range + min_range);
	double new_range_deg = rad2deg( 0.5*(max_range - min_range) );

	double x_new[3];
	int num_correspondences=-1;
	ght_one_shot(laser_ref, laser_sens,
			u, params->max_linear_correction*2,
			new_range_deg, interval, x_new, &num_correspondences) ;

	if(num_correspondences < laser_ref->nrays) {
		sm_error("sm_gpm(): I found only %d correspondences in the second pass of GPM. I consider it a failure.\n",
			num_correspondences);
		return;
	}

	/* Et voila, in x_new we have the answer */

	{
		sm_debug("gpm : max_correction_lin %f def %f\n", params->max_linear_correction, 		params->max_angular_correction_deg);
		sm_debug("gpm : acceptable range for theta: [%f, %f]\n", min_range,max_range);
		sm_debug("gpm : 1) Num correspondences for theta: %d\n", num_correspondences_theta);

		sm_debug("gpm 1/2: new u = : %s \n", friendly_pose(u) );
		sm_debug("gpm 1/2: New range: %f to %f\n",rad2deg(min_range),rad2deg(max_range));

		sm_debug("gpm 2/2: Solution: %s \n", friendly_pose(x_new));
	/*	if(jf()) fprintf(jf(), "iteration 2\n");
		journal_pose("x_old", x_new);	*/
	}

	/* Administrivia */

	res->valid = 1;
	copy_d(x_new, 3, res->x);

	res->iterations = 0;

	gsl_histogram_free(hist);
}

void ght_find_theta_range(LDP laser_ref, LDP laser_sens,
	const double*x0, double max_linear_correction,
	double max_angular_correction_deg, int interval, gsl_histogram*hist, int*num_correspondences)
{
	/** Compute laser_sens's points in laser_ref's coordinates by roto-translating by x0 */
	ld_compute_world_coords(laser_sens, x0);

	int count = 0;
	int i;
	for(i=0;i<laser_sens->nrays;i++) {
		if(!laser_sens->alpha_valid[i]) continue;
		if(i % interval) continue;

		const double * p_i = laser_sens->points[i].p;

		const double * p_i_w = laser_sens->points_w[i].p;
		int from; int to; int start_cell;
		possible_interval(p_i_w, laser_ref, max_angular_correction_deg,
			max_linear_correction, &from, &to, &start_cell);

//		printf("\n i=%d interval = [%d,%d] ", i, from, to);
		int j;
		for(j=from;j<=to;j++) {
			if(!laser_ref->alpha_valid[j]) continue;
			if(j % interval) continue;

			double theta = angleDiff(laser_ref->alpha[j], laser_sens->alpha[i]);
			double theta_diff = angleDiff(theta,x0[2]);
			if( fabs(theta_diff) > deg2rad(max_angular_correction_deg) )
				continue;
			theta = x0[2] + theta_diff; // otherwise problems near +- PI

			const double * p_j = laser_ref->points[j].p;

			double c = cos(theta); double s = sin(theta);
			double t_x = p_j[0] - (c*p_i[0]-s*p_i[1]);
			double t_y = p_j[1] - (s*p_i[0]+c*p_i[1]);
			double t_dist = sqrt( square(t_x-x0[0]) + square(t_y-x0[1]) );

			if(t_dist > max_linear_correction)
				continue;

			/*double weight = 1/(laser_sens->cov_alpha[i]+laser_ref->cov_alpha[j]);*/
			double weight = 1;
			gsl_histogram_accumulate(hist, theta, weight);
			gsl_histogram_accumulate(hist, theta+2*M_PI, weight); /* be robust */
			gsl_histogram_accumulate(hist, theta-2*M_PI, weight);
			count ++;
		}
	}
	*num_correspondences = count;
	sm_debug(" correspondences = %d\n",count);
}

void ght_one_shot(LDP laser_ref, LDP laser_sens,
		const double*x0, double max_linear_correction,
	double max_angular_correction_deg, int interval, double*x, int*num_correspondences)
{
	/** Compute laser_sens's points in laser_ref's coordinates by roto-translating by x0 */
	ld_compute_world_coords(laser_sens, x0);

	double L[3][3]  = {{0,0,0},{0,0,0},{0,0,0}};
	double z[3] = {0,0,0};

	int count = 0;
	int i;
	for(i=0;i<laser_sens->nrays;i++) {
		if(!laser_sens->alpha_valid[i]) continue;
		if(i % interval) continue;

		const double * p_i = laser_sens->points_w[i].p;

		const double * p_i_w = laser_sens->points_w[i].p;
		int from; int to; int start_cell;
		possible_interval(p_i_w, laser_ref, max_angular_correction_deg,
			max_linear_correction, &from, &to, &start_cell);
//		from = 0; to = laser_ref->nrays-1;

		int j;
		for(j=from;j<=to;j++) {
			if(j % interval) continue;
			if(!laser_ref->alpha_valid[j]) continue;

			double theta = angleDiff(laser_ref->alpha[j], laser_sens->alpha[i]);
			double theta_diff = angleDiff(theta,x0[2]);
			if( fabs(theta_diff) > deg2rad(max_angular_correction_deg) )
				continue;
			theta = x0[2] + theta_diff; // otherwise problems near +- PI

			const double * p_j = laser_ref->points[j].p;

			double c = cos(theta); double s = sin(theta);
			double t_x = p_j[0] - (c*p_i[0]-s*p_i[1]);
			double t_y = p_j[1] - (s*p_i[0]+c*p_i[1]);
			double t_dist = sqrt( square(t_x-x0[0]) + square(t_y-x0[1]) );

			if(t_dist > max_linear_correction)
				continue;

			/*double weight = 1/(laser_sens->cov_alpha[i]+laser_ref->cov_alpha[j]);
			double weight = exp( -square(t_dist) - 5 * square(theta-x0[2]) );*/

			double weight = 1;

			double alpha = laser_ref->alpha[j];
			double ca = cos(alpha); double sa=sin(alpha);

//				printf("%d ", (int) rad2deg(theta));

/*			printf("valid %d alpha %f weight %f t_x %f t_y %f\n",
				laser_ref->alpha_valid[j],alpha,weight,
				t_x, t_y); */
			z[0] += weight*(ca*ca*t_x + sa*ca*t_y);
			z[1] += weight*(sa*ca*t_x + sa*sa*t_y);
			z[2] += weight*theta;
			L[0][0] += weight* ca * ca;
			L[0][1] += weight* sa * ca;
			L[1][0] += weight* sa * ca;
			L[1][1] += weight* sa * sa;
			L[2][2] += weight;

			count += 1;
		}
	}

	*num_correspondences = count;

	if(1) {

		double weight = 0.5 * count;
		z[0] += x0[0] * weight;
		z[1] += x0[1] * weight;
		L[0][0] += weight;
		L[0][1] += 0;
		L[1][0] += 0;
		L[1][1] += weight;
	}

	egsl_push();
		val eL = egsl_alloc(3,3);
			size_t a,b;
			for(a=0;a<3;a++)
			for(b=0;b<3;b++)
				*egsl_atmp(eL,a,b) = L[a][b];

/*		egsl_print("eL", eL);*/
		val ez = egsl_vFa(3,z);

		val ex = m(inv(eL), ez);

		egsl_v2a(ex, x);

/*		egsl_print("eL", eL);
		egsl_print("ez", ez);
		egsl_print("ex", ex); */

	egsl_pop();

//	sm_debug("gpm: second step: theta = %f   %f / %d = %f \n", rad2deg(x[2]), rad2deg(z[2]), count, rad2deg(z[2]) / count);
	sm_debug("gpm: second step: found %d correspondences\n",count);

}


text:
/*
 Example usage of MIXMAX with GSL

 *  Copyright Konstantin Savvidy.
 *
 *  Free to use, academic or commercial. Do not redistribute without permission.
 *
 *	G.K.Savvidy and N.G.Ter-Arutyunian,
 *  On the Monte Carlo simulation of physical systems,
 *	J.Comput.Phys. 97, 566 (1991);
 *  Preprint EPI-865-16-86, Yerevan, Jan. 1986
 *
 *  K.Savvidy
 *  The MIXMAX random number generator
 *  Comp. Phys. Commun. 196 (2015), pp 161–165
 *  http://dx.doi.org/10.1016/j.cpc.2015.06.003
 *
 */

// cc -std=c99 -O3 -funroll-loops -Wall -I /usr/local/include/ driver_gsl.c mixmax.o -L/usr/local/lib -DHOOKUP_GSL=1  -o gsl -lgsl
// make gsl; GSL_RNG_SEED=123 GSL_RNG_TYPE=MIXMAX ./gsl

#include <stdio.h>
#include "mixmax.h"
#include <gsl/gsl_rng.h>
#include <gsl/gsl_version.h>

int
main (void)
{
  const gsl_rng_type * T;
  gsl_rng * r;
  unsigned long int seed = 0;

  int i, n = 10;

 // gsl_rng_env_setup();
 // gsl_rng_types_setup ();

  T = gsl_rng_mixmax;

    const char *p = getenv ("GSL_RNG_SEED");
    if (p)
    {
        seed = strtoul (p, 0, 0);
        fprintf (stderr, "GSL_RNG_SEED=%lu\n", seed);
    };

    gsl_rng_default_seed = seed;

    r = gsl_rng_alloc (T);

    printf("generator type: %s\n", gsl_rng_name (r));
    printf("seed = %lu\n", gsl_rng_default_seed);
    printf("gsl_version=%s\n", gsl_version);

  for (i = 0; i < n; i++)
    {
      double u = gsl_rng_uniform (r);
      printf ("%.16F\n", u);
    }

  gsl_rng_free (r);

  return 0;
}

max_stars_repo_name: ruicamposcolabpt/MontyCarlo

max_issues_repo_name: ruicamposcolabpt/MontyCarlo

max_forks_repo_name: ruicamposcolabpt/MontyCarlo


text:
/************************************************************************
 Dataset.h.h - Copyright marcello

 Here you can write a license for your code, some comments or any other
 information you want to have in your generated code. To to this simply
 configure the "headings" directory in uml to point to a directory
 where you have your heading files.

 or you can just replace the contents of this file with your own.
 If you want to do this, this file is located at

 /usr/share/apps/umbrello/headings/heading.h

 -->Code Generators searches for heading files based on the file extension
 i.e. it will look for a file name ending in ".h" to include in C++ header
 files, and for a file name ending in ".java" to include in all generated
 java code.
 If you name the file "heading.<extension>", Code Generator will always
 choose this file even if there are other files with the same extension in the
 directory. If you name the file something else, it must be the only one with that
 extension in the directory to guarantee that Code Generator will choose it.

 you can use variables in your heading files which are replaced at generation
 time. possible variables are : author, date, time, filename and filepath.
 just write %variable_name%

 This file was generated on Sat Nov 10 2007 at 15:05:38
 The original location of this file is /home/marcello/Projects/fitted/Developing/Dataset.h
 **************************************************************************/

#ifndef DATASET_H
#define DATASET_H

#include <vector>
#include <set>
#include <map>

//#include <gsl/gsl_matrix.h>

#include "Sample.h"

//<< Sam
namespace PoliFitted
{

typedef enum
{
  OVERWRITE,
  APPEND
} ModalityType;
typedef enum
{
  EUCLIDEAN,
  MANHATTAN,
  MAHALANOBIS
} MetricType;

/**
 * Represents an in-memory cache of numeric data.
 * A Dataset is nothing more than what its name implies: a set of data.
 * Since each data is a sample, the dataset can be split into input and
 * output data. In contrast with the Sample class, the
 * dataset stores the information related to the dimension of the input and output
 * tuples.
 *
 * Advanced features include iterator, random subsampling, normalization,
 * statistical data, import and export.
 *
 * @brief In-memory cache of numeric data.
 */
class Dataset: public vector<Sample*>
{
  public:

    // Constructors/Destructors
    //

    /**
     * Empty Constructor
     */
    Dataset(unsigned int input_size = 1, unsigned int output_size = 1);

    /**
     * Empty Destructor
     */
    virtual ~Dataset();

    /**
     * Destroys the content of the dataset but not the dataset itself.
     *
     * The input data of each sample are destroyed only if \a clear_input
     * is set to true.
     *
     * @brief Clear tuple
     * @param clear_input If true the input data are destrayed, false otherwise.
     */
    void Clear(bool clear_input = false);

    /**
     * @brief Set input data dimension
     * @param size The dimension of the input data.
     */
    void SetInputSize(unsigned int size);

    /**
     * @brief Set output data dimension
     * @param size The dimension of the output data.
     */
    void SetOutputSize(unsigned int size);

    /**
     * @brief Return the input dimension.
     * @return The dimension of the input component of each sample.
     */
    unsigned int GetInputSize();

    /**
     * @brief Return the output dimension.
     * @return The dimension of the output component of each sample.
     */
    unsigned int GetOutputSize();

    /**
     * Adds a new element at the end of the dataset, after its current last element.
     * This effectively increases the container size by one, which causes an automatic
     * allocation of a new samples initialized with the given input and output tuples.
     *
     * @brief Add element at the end.
     * @param input The input tuple to be added.
     * @param output The output tuple to be added.
     */
    void AddSample(Tuple* input, Tuple* output);

    /**
     * Returns a pointer to a newly created memory array storing
     * the elements of the dataset.
     * Data are stored linearly in the new array \f$A\f$. For any
     * sample in the dataset, input elements are stored before
     * output elements. As a consequence the dimension of the
     * array \f$A\f$ is \f$n_{el} * (input_dim + output_dim)\f$.
     * \f[ A = {input_i(1),\dots,input_i(m), output_i(1),\dots,output_i(n)}_{i=0}^{n_{el}-1}\f]
     *
     * @brief Get a copy of the elements.
     * @return A reference to the first element of an array storing
     * the dataset elements
     */
    //double* GetMatrix();
    /**
     * Returns a pointer to a newly created GSL matrix storing
     * the elements of the dataset. Each row represents a sample
     * of the dataset. For any
     * sample in the dataset, input elements are stored before
     * output elements. As a consequence the dimension of the
     * array \f$A\f$ is \f$n_{el} \times (input_dim + output_dim)\f$.
     * The element M(i,j) corresponds to the i-th sample of the dataset,
     * if j is less then input_dimension corresponds to the j-th element
     * of the input tuple. If j is greater or equal than the input_dimension,
     * M(i,j) corresponds to the (j-input_dimension) element of the output
     * tuple.
     *
     * @brief Get a copy of the elements.
     * @return A GSL matrix storing the dataset elements
     */
    //gsl_matrix* GetGSLMatrix();
    /**
     *
     */
    //gsl_matrix* GetCovarianceMatrix();
    /**
     * Clone the current instance filling the new dataset with
     * a clone of each sample. This function performs a deep copy.
     *
     * @brief Clone object
     * @return A newly created dataset.
     *
     * @see Sample::Clone
     */
    Dataset* Clone();

    /**
     * @brief Resize the output tuple of each sample
     * @param new_size the new size of the output tuple
     */
    void ResizeOutput(unsigned int new_size);

    /**
     *
     */
    Dataset* GetReducedDataset(unsigned int size, bool random = false);

    /**
     *
     */
    Dataset* GetReducedDataset(float proportion, bool random = false);

    /**
     *
     */
    void GetTrainAndTestDataset(unsigned int num_partitions, unsigned int partition,
        Dataset* train_ds, Dataset* test_ds);

    /**
     *
     */
    vector<Dataset*> SplitDataset(unsigned int parts);

    /**
     *
     */
    map<float, Dataset*> SplitByAttribute(unsigned int attribute);

    /**
     *
     */
    Dataset* ExtractNewDataset(set<unsigned int> inputs, set<unsigned int> outputs);

    /**
     *
     */
    Sample* GetNearestNeighbor(Tuple& input, MetricType metric = EUCLIDEAN);

    /**
     *
     */
    void Save(string filename, ModalityType modality = OVERWRITE);

    /**
     *
     */
    void Load(string filename);

    /**
     *
     */
    Dataset* NormalizeMinMax(vector<pair<float, float> >& input_parameters,
        vector<pair<float, float> >& output_parameters);

    /**
     *
     */
    Dataset* NormalizeOutputMinMax(vector<pair<float, float> >& output_parameters);

    /**
     * @brief Mean of the first output element.
     * @return The mean value of the first output element.
     */
    float Mean();

    /**
     * @brief Variance of the first output element.
     * @return The variance value of the first output element.
     */
    float Variance();

    /**
     * Insert a formatted representation of the dataset into the given output stream.
     *
     * @brief Insert formatted output
     * @param out Output stream object where characters are inserted.
     * @param ds Dataset to be printed out
     * @return The reference to the stream it receives
     */
    friend ofstream& operator<<(ofstream& out, PoliFitted::Dataset& ds);

    /**
     * Extract formatted input from the stream and fill the dataset
     * accordingly to that information.
     *
     * @brief Extract formatted input
     * @param in Input stream object from which characters are extracted.
     * @param ds where the extracted information are stored.
     * @return The reference to the stream it receives
     */
    friend ifstream& operator>>(ifstream& in, PoliFitted::Dataset& ds);

  protected:

  private:
    unsigned int mInputSize;
    unsigned int mOutputSize;
    float mMean;
    float mVariance;

    /**
     * @brief Compute mean and variance of the first output element.
     */
    void ComputeMeanVariance();

};

inline void Dataset::SetInputSize(unsigned int size)
{
  mInputSize = size;
}

inline void Dataset::SetOutputSize(unsigned int size)
{
  mOutputSize = size;
}

inline unsigned int Dataset::GetInputSize()
{
  return mInputSize;
}

inline unsigned int Dataset::GetOutputSize()
{
  return mOutputSize;
}

inline void Dataset::AddSample(Tuple* input, Tuple* output)
{
  push_back(new Sample(input, output));
}

inline void Dataset::Save(string filename, ModalityType modality)
{
  ofstream log_file;
  if (modality == OVERWRITE)
  {
    log_file.open(filename.c_str(), ios::out);
  }
  else
  {
    log_file.open(filename.c_str(), ios::out | ios::app);
  }
  if (log_file.is_open())
  {
    log_file << *(this);
  }
  log_file.close();
}

inline void Dataset::Load(string filename)
{
  ifstream log_file;
  log_file.open(filename.c_str(), ios::in);
  log_file >> *(this);
  log_file.close();
}

}

#endif // DATASET_H

max_stars_repo_name: akangasr/sdirl

max_issues_repo_name: akangasr/sdirl

max_forks_repo_name: akangasr/sdirl


text:
/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * 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.
 */

#pragma once

#ifndef dvt_31727ef2_2dfa_4e64_b93d_deab9c280036_h
#define dvt_31727ef2_2dfa_4e64_b93d_deab9c280036_h

#include <gslib/config.h>
#include <gslib/type.h>
#include <gslib/std.h>

__gslib_begin__

template<class _targetfn>
inline uint method_address(_targetfn fn)
{
    uint r;
    memcpy(&r, &fn, sizeof(r));
    return r;
}

extern uint dsm_get_address(uint addr, uint pthis);
extern void dvt_recover_vtable(void* pthis, void* ovt, int vtsize);

class dvt_bridge_code;

template<class _cls>
class vtable_ops:
    public _cls
{
public:
    typedef vtable_ops<_cls> myref;

public:
    myref() {}
    template<class _arg1>
    myref(_arg1 a1): _cls(a1) {}
    template<class _arg1, class _arg2>
    myref(_arg1 a1, _arg2 a2): _cls(a1, a2) {}
    template<class _arg1, class _arg2, class _arg3>
    myref(_arg1 a1, _arg2 a2, _arg3 a3): _cls(a1, a2, a3) {}
    virtual void end_of_vtable() { __asm { nop } }      /* add some real stuff in this function to prevent optimization in release. */

public:
    int get_virtual_method_index(uint m) const
    {
        uint mv = dsm_final_address(m);
        uint eov = dsm_final_address(method_address(&myref::end_of_vtable));
        uint* ovt = *(uint**)this;
        int i = 0;
        for(;; ++ i) {
            uint cv = dsm_final_address(ovt[i]);
            if(cv == mv)
                return i;
            if(cv == eov) {
                assert(!"get index failed.");
                return i;
            }
        }
        return -1;
    }
    int size_of_vtable() const
    {
        return get_virtual_method_index(
            dsm_final_address(method_address(&myref::end_of_vtable))
            );
    }
    void* create_per_instance_vtable(_cls* p, dvt_bridge_code& bridges)
    {
        assert(p);
        int count = size_of_vtable();
        byte* ovt = *(byte**)p;
        void** pvt = (void**)VirtualAlloc(nullptr, count * 4, MEM_COMMIT, PAGE_EXECUTE_READWRITE);
        assert(pvt);
        bridges.install(ovt, count);
        for(int i = 0; i < count; i ++)
            pvt[i] = bridges.get_bridge(i);
        memcpy(p, &pvt, 4);
        DWORD oldpro;
        VirtualProtect(pvt, count * 4, PAGE_EXECUTE_READ, &oldpro);
        return ovt;
    }
    void destroy_per_instance_vtable(_cls* p, void* ovt)
    {
        assert(p && ovt);
        dvt_recover_vtable(p, ovt, size_of_vtable());
    }
    template<class _func>
    uint replace_vtable_method(_cls* p, int index, _func func)
    {
        uint* vt = *(uint**)p;
        vt += index;
        uint r = *vt;
        DWORD oldpro;
        VirtualProtect(vt, 4, PAGE_EXECUTE_READWRITE, &oldpro);
        *vt = (uint)func;
        VirtualProtect(vt, 4, oldpro, &oldpro);
        return r;
    }

private:
    uint dsm_final_address(uint addr) const
    {
        return dsm_get_address(
            addr, (uint)((void*)this)
        );
    }
};

class dvt_detour_code
{
    typedef unsigned long ulong;

public:
    enum detour_type
    {
        detour_notify,
        detour_reflect,
    };

public:
    dvt_detour_code(detour_type dty, int argsize);
    ~dvt_detour_code();
    void finalize(uint old_func, uint host, uint action);
    uint get_code_address() const { return (uint)_ptr; }

private:
    detour_type     _type;
    int             _argsize;
    byte*           _ptr;
    int             _len;
    ulong           _oldpro;
};

/*
 * The two kind of DVT callings:
 * 1.notify:    the original function would be called first, and then the notify function will be called after, you MUST specify the size of the argument table.
 * 2.reflect:   the original function would NOT be called, simply jump to the reflect function, and the argument table of the reflect function SHOULD be exactly the same with the original function.
 */
#define connect_typed_detour(targettype, target, trigger, host, action, dty, argsize) { \
    auto& vo = vtable_ops<targettype>(nullptr); \
    (target)->ensure_dvt_available<targettype>(); \
    auto* detour = (target)->add_detour(dty, argsize); \
    assert(detour); \
    uint old_func = vo.replace_vtable_method(target, vo.get_virtual_method_index(method_address(trigger)), detour->get_code_address()); \
    detour->finalize(old_func, (uint)host, method_address(action)); \
}

#define connect_typed_notify(targettype, target, trigger, host, action, argsize) \
    connect_typed_detour(targettype, target, trigger, host, action, dvt_detour_code::detour_notify, argsize)

#define connect_notify(target, trigger, host, action, argsize) \
    connect_typed_notify(std::remove_reference_t<decltype(*target)>, target, trigger, host, action, argsize)

#define connect_typed_reflect(targettype, target, trigger, host, action) \
    connect_typed_detour(targettype, target, trigger, host, action, dvt_detour_code::detour_reflect, 0)

#define connect_reflect(target, trigger, host, action) \
    connect_typed_reflect(std::remove_reference_t<decltype(*target)>, target, trigger, host, action)

/*
 * Bridge to jump to the original vtable
 * The reason why we can't simply copy the original vtable was that it's hard to determine how long the vtable actually was.
 * If the target class had a complex derivations, missing a part of the vtable would make you unable to call __super::function.
 */
class dvt_bridge_code
{
public:
    dvt_bridge_code();
    ~dvt_bridge_code();
    void install(void* vt, int count);
    void* get_bridge(int index) const;

protected:
    byte*           _bridges;
    int             _bridge_size;       /* in bytes */
    int             _jt_stride;         /* jump table stride in bytes */
};

class dvt_holder
{
public:
    typedef vector<dvt_detour_code*> detour_list;

public:
    dvt_holder();
    virtual ~dvt_holder();

private:
    void*           _backvt;
    int             _backvtsize;
    void*           _switchvt;
    detour_list     _detours;
    bool            _delete_later;
    dvt_bridge_code _bridges;

public:
    template<class _cls>
    void ensure_dvt_available()
    {
        if(_backvt)
            return;
        auto& vo = vtable_ops<_cls>(nullptr);
        _backvtsize = vo.size_of_vtable();
        _backvt = vo.create_per_instance_vtable(static_cast<_cls*>(this), _bridges);
    }
    dvt_detour_code* add_detour(dvt_detour_code::detour_type dty, int argsize);
    dvt_holder* switch_to_ovt();        /* switch to original vtable */
    dvt_holder* switch_to_dvt();
    /*
     * In case the holder would be tagged more than once by the garbage collector, the cause might be
     * a message re-entrant of the msg callback function. So here we tag it.
     */
    void set_delete_later() { _delete_later = true; }
    bool is_delete_later() const { return _delete_later; }
};

class dvt_collector
{
public:
    typedef vector<dvt_holder*> holder_list;

private:
    holder_list     _holders;
    dvt_collector() {}

public:
    static dvt_collector* get_singleton_ptr()
    {
        static dvt_collector inst;
        return &inst;
    }
    ~dvt_collector() { cleanup(); }
    bool set_delete_later(dvt_holder* holder);
    void cleanup();
};

__gslib_end__

#endif

max_stars_repo_name: lymastee/gslib

max_issues_repo_name: lymastee/gslib

max_forks_repo_name: lymastee/gslib


text:
/* rng/rng-dump.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 James Theiler, Brian Gough
 *
 * 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 <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

int
main (int argc, char **argv)
{
  int i, j ;
  char buffer[1024 * 4] ;
  gsl_rng * r ;

  gsl_rng_env_setup () ;

  r = gsl_rng_alloc (gsl_rng_default) ;

  if (argc != 1)
    {
      printf ("Usage: diehard\n");
      printf ("Output 3 million numbers in binary format,"
              "suitable for testing with DIEHARD\n");
      exit (0);
    }

  argv = 0 ; /* prevent warning about unused argument */

  for (i = 0; i < 3000 ; i++)
    {
      int status ;

      for (j = 0; j < 1024; j++)
        {
          unsigned long int u = gsl_rng_get (r) ;
          buffer[4 * j + 0] = u & 0xFF ;
          u >>= 8;
          buffer[4 * j + 1] = u & 0xFF ;
          u >>= 8;
          buffer[4 * j + 2] = u & 0xFF ;
          u >>= 8;
          buffer[4 * j + 3] = u & 0xFF ;

        }

      status = fwrite(buffer, 4 * sizeof(char), 1024, stdout) ;

      if (status != 1024)
        {
          perror("fwrite") ;
          exit(EXIT_FAILURE) ;
        }
    }

  return 0;
}

max_stars_repo_name: zzpwahaha/Chimera-Control-Trim

max_issues_repo_name: zzpwahaha/Chimera-Control-Trim

max_forks_repo_name: zzpwahaha/Chimera-Control-Trim


text:
/**
 *
 * @file core_slarfb_gemm.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Azzam Haidar
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:44:49 2014
 *
 **/
#include <cblas.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_float
 *
 *  CORE_slarfb_gemm applies a complex block reflector H or its transpose H'
 *  to a complex M-by-N matrix C, from either the left or the right.
 *  this kernel is similar to the lapack slarfb but it do a full gemm on the
 *  triangular Vs assuming that the upper part of Vs is zero and ones are on
 *  the diagonal. It is also based on the fact that a gemm on a small block of
 *  k reflectors is faster than a trmm on the triangular (k,k) + gemm below.
 *
 *  NOTE THAT:
 *  Only Columnwise/Forward cases are treated here.
 *
 *******************************************************************************
 *
 * @param[in] side
 *         @arg PlasmaLeft  : apply Q or Q**T from the Left;
 *         @arg PlasmaRight : apply Q or Q**T from the Right.
 *
 * @param[in] trans
 *         @arg PlasmaNoTrans   : No transpose, apply Q;
 *         @arg PlasmaTrans : ConjTranspose, apply Q**T.
 *
 * @param[in] direct
 *         Indicates how H is formed from a product of elementary
 *         reflectors
 *         @arg PlasmaForward  : H = H(1) H(2) . . . H(k) (Forward)
 *         @arg PlasmaBackward : H = H(k) . . . H(2) H(1) (Backward)
 *
 * @param[in] storev
 *         Indicates how the vectors which define the elementary
 *         reflectors are stored:
 *         @arg PlasmaColumnwise
 *         @arg PlasmaRowwise
 *
 * @param[in] M
 *         The number of rows of the matrix C.
 *
 * @param[in] N
 *         The number of columns of the matrix C.
 *
 * @param[in] K
 *          The order of the matrix T (= the number of elementary
 *          reflectors whose product defines the block reflector).
 *
 * @param[in] V
 *          COMPLEX*16 array, dimension
 *              (LDV,K) if storev = 'C'
 *              (LDV,M) if storev = 'R' and side = 'L'
 *              (LDV,N) if storev = 'R' and side = 'R'
 *          The matrix V. See further details.
 *
 * @param[in] LDV
 *         The leading dimension of the array V.
 *         If storev = 'C' and side = 'L', LDV >= max(1,M);
 *         if storev = 'C' and side = 'R', LDV >= max(1,N);
 *         if storev = 'R', LDV >= K.
 *
 * @param[in] T
 *         The triangular K-by-K matrix T in the representation of the
 *         block reflector.
 *         T is upper triangular by block (economic storage);
 *         The rest of the array is not referenced.
 *
 * @param[in] LDT
 *         The leading dimension of the array T. LDT >= K.
 *
 * @param[in,out] C
 *         COMPLEX*16 array, dimension (LDC,N)
 *         On entry, the M-by-N matrix C.
 *         On exit, C is overwritten by H*C or H'*C or C*H or C*H'.
 *
 * @param[in] LDC
 *         The leading dimension of the array C. LDC >= max(1,M).
 *
 * @param[in,out] WORK
 *         (workspace) COMPLEX*16 array, dimension (LDWORK,K).
 *
 * @param[in] LDWORK
 *         The dimension of the array WORK.
 *         If side = PlasmaLeft,  LDWORK >= max(1,N);
 *         if side = PlasmaRight, LDWORK >= max(1,M).
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_slarfb_gemm = PCORE_slarfb_gemm
#define CORE_slarfb_gemm PCORE_slarfb_gemm
#endif
int CORE_slarfb_gemm(PLASMA_enum side, PLASMA_enum trans, int direct, int storev,
                     int M, int N, int K,
                     const float *V, int LDV,
                     const float *T, int LDT,
                     float *C, int LDC,
                     float *WORK, int LDWORK)
{
    static float zzero =  0.0;
    static float zone  =  1.0;
    static float mzone = -1.0;
    /* Check input arguments */
    if ((side != PlasmaLeft) && (side != PlasmaRight)) {
        coreblas_error(1, "Illegal value of side");
        return -1;
    }
    if ((trans != PlasmaNoTrans) && (trans != PlasmaTrans)) {
        coreblas_error(2, "Illegal value of trans");
        return -2;
    }
    if ((direct != PlasmaForward) && (direct != PlasmaBackward)) {
        coreblas_error(3, "Illegal value of direct");
        return -3;
    }
    if ((storev != PlasmaColumnwise) && (storev != PlasmaRowwise)) {
        coreblas_error(4, "Illegal value of direct");
        return -4;
    }
    if (M < 0) {
        coreblas_error(5, "Illegal value of M");
        return -5;
    }
    if (N < 0) {
        coreblas_error(6, "Illegal value of N");
        return -6;
    }
    if (K < 0) {
        coreblas_error(7, "Illegal value of K");
        return -7;
    }

    /* Quick return */
    if ((M == 0) || (N == 0) || (K == 0) )
        return PLASMA_SUCCESS;

    /* For Left case, switch the trans. noswitch for right case */
    if( side == PlasmaLeft){
        if ( trans == PlasmaNoTrans) {
            trans = PlasmaTrans;
        }
        else {
            trans = PlasmaNoTrans;
        }
    }

    /* main code */
    if (storev == PlasmaColumnwise )
    {
        if ( direct == PlasmaForward )
        {
            /*
             * Let  V =  ( V1 )    (first K rows are unit Lower triangular)
             *           ( V2 )
             */
            if ( side == PlasmaLeft )
            {
                /*
                 * Columnwise / Forward / Left
                 */
                /*
                 * Form  H * C  or  H' * C  where  C = ( C1 )
                 *                                     ( C2 )
                 *
                 * W := C' * V    (stored in WORK)
                 */
                cblas_sgemm(
                    CblasColMajor, CblasTrans, CblasNoTrans,
                    N, K, M,
                    (zone), C, LDC,
                    V, LDV,
                    (zzero), WORK, LDWORK );
                /*
                 * W := W * T'  or  W * T
                 */
                cblas_strmm(
                    CblasColMajor, CblasRight, CblasUpper,
                    (CBLAS_TRANSPOSE)trans, CblasNonUnit, N, K,
                    (zone), T, LDT, WORK, LDWORK );
                /*
                 * C := C - V * W'
                 */
                cblas_sgemm(
                    CblasColMajor, CblasNoTrans, CblasTrans,
                    M, N, K,
                    (mzone), V, LDV,
                    WORK, LDWORK,
                    (zone), C, LDC);
            }
            else {
                /*
                 * Columnwise / Forward / Right
                 */
                /*
                 * Form  C * H  or  C * H'  where  C = ( C1  C2 )
                 * W := C * V
                 */
                cblas_sgemm(
                    CblasColMajor, CblasNoTrans, CblasNoTrans,
                    M, K, N,
                    (zone), C, LDC,
                    V, LDV,
                    (zzero), WORK, LDWORK);
                /*
                 * W := W * T  or  W * T'
                 */
                cblas_strmm(
                    CblasColMajor, CblasRight, CblasUpper,
                    (CBLAS_TRANSPOSE)trans, CblasNonUnit, M, K,
                    (zone), T, LDT, WORK, LDWORK );
                /*
                 * C := C - W * V'
                 */
                cblas_sgemm(
                    CblasColMajor, CblasNoTrans, CblasTrans,
                    M, N, K,
                    (mzone), WORK, LDWORK,
                    V, LDV,
                    (zone), C, LDC);
            }
        }
        else {
            /*
             * Columnwise / Backward / Left or Right
             */
            coreblas_error(3, "Not implemented (ColMajor / Backward / Left or Right)");
            return PLASMA_ERR_NOT_SUPPORTED;
        }
    }
    else {
        if (direct == PlasmaForward) {
            /*
             * Rowwise / Forward / Left or Right
             */
            coreblas_error(3, "Not implemented (RowMajor / Backward / Left or Right)");
            return PLASMA_ERR_NOT_SUPPORTED;
        }
        else {
            /*
             * Rowwise / Backward / Left or Right
             */
            coreblas_error(3, "Not implemented (RowMajor / Backward / Left or Right)");
            return PLASMA_ERR_NOT_SUPPORTED;
        }
    }
    return PLASMA_SUCCESS;
}


text:
#include <math.h>
#include <gsl/gsl_math.h>
#include <egsl/egsl_macros.h>

#include "icp.h"
#include "../csm_all.h"

val compute_C_k(val p_j1, val p_j2);
val dC_drho(val p1, val p2);

void compute_covariance_exact(
	LDP laser_ref, LDP laser_sens, const gsl_vector*x,
		val *cov0_x, val *dx_dy1, val *dx_dy2)
{
	egsl_push_named("compute_covariance_exact");

	val d2J_dxdy1 = zeros(3,(size_t)laser_ref ->nrays);
	val d2J_dxdy2 = zeros(3,(size_t)laser_sens->nrays);

	/* the three pieces of d2J_dx2 */
	val d2J_dt2       = zeros(2,2);
	val d2J_dt_dtheta = zeros(2,1);
	val d2J_dtheta2   = zeros(1,1);

	double theta = x->data[2];
	val t = egsl_vFa(2,x->data);

	int i;
	for(i=0;i<laser_sens->nrays;i++) {
		if(!ld_valid_corr(laser_sens,i)) continue;
		egsl_push_named("compute_covariance_exact iteration");

		int j1 = laser_sens->corr[i].j1;
		int j2 = laser_sens->corr[i].j2;

		val p_i  = egsl_vFa(2, laser_sens->points[i].p);
		val p_j1 = egsl_vFa(2, laser_ref ->points[j1].p);
		val p_j2 = egsl_vFa(2, laser_ref ->points[j2].p);

		/* v1 := rot(theta+M_PI/2)*p_i */
		val v1 = m(rot(theta+M_PI/2), p_i);
		/* v2 := (rot(theta)*p_i+t-p_j1) */
		val v2 = sum3( m(rot(theta),p_i), t, minus(p_j1));
		/* v3 := rot(theta)*v_i */
		val v3 = vers(theta + laser_sens->theta[i]);
		/* v4 := rot(theta+PI/2)*v_i; */
		val v4 = vers(theta + laser_sens->theta[i] + M_PI/2);

		val C_k = compute_C_k(p_j1, p_j2);

		val d2J_dt2_k = sc(2.0, C_k);
		val d2J_dt_dtheta_k = sc(2.0,m(C_k,v1));

		val v_new = m(rot(theta+M_PI), p_i);
		val d2J_dtheta2_k = sc(2.0, sum( m3(tr(v2),C_k, v_new), m3(tr(v1),C_k,v1)));
		add_to(d2J_dt2, d2J_dt2_k);
		add_to(d2J_dt_dtheta, d2J_dt_dtheta_k );
		add_to(d2J_dtheta2, d2J_dtheta2_k);

		/* for measurement rho_i  in the second scan */
		val d2Jk_dtdrho_i = sc(2.0, m(C_k,v3));
		val d2Jk_dtheta_drho_i = sc(2.0, sum( m3(tr(v2),C_k,v4),  m3(tr(v3),C_k,v1)));
 		add_to_col(d2J_dxdy2, (size_t)i, comp_col(d2Jk_dtdrho_i, d2Jk_dtheta_drho_i));

		/* for measurements rho_j1, rho_j2 in the first scan */

		val dC_drho_j1 = dC_drho(p_j1, p_j2);
		val dC_drho_j2 = dC_drho(p_j2, p_j1);

		val v_j1 = vers(laser_ref->theta[j1]);

		val d2Jk_dt_drho_j1 = sum(sc(-2.0,m(C_k,v_j1)), sc(2.0,m(dC_drho_j1,v2)));
		val d2Jk_dtheta_drho_j1 = sum( sc(-2.0, m3(tr(v_j1),C_k,v1)), m3(tr(v2),dC_drho_j1,v1));
		add_to_col(d2J_dxdy1, (size_t)j1, comp_col(d2Jk_dt_drho_j1, d2Jk_dtheta_drho_j1));

		/* for measurement rho_j2*/
		val d2Jk_dt_drho_j2 = sc(2.0, m( dC_drho_j2,v2));
		val d2Jk_dtheta_drho_j2 = sc(2.0, m3( tr(v2), dC_drho_j2, v1));
		add_to_col(d2J_dxdy1, (size_t)j2, comp_col(d2Jk_dt_drho_j2, d2Jk_dtheta_drho_j2));

		egsl_pop_named("compute_covariance_exact iteration");
	}

	/* composes matrix  d2J_dx2  from the pieces*/
	val d2J_dx2   = comp_col( comp_row(    d2J_dt2      ,   d2J_dt_dtheta),
	                          comp_row(tr(d2J_dt_dtheta),     d2J_dtheta2));

	val edx_dy1 = sc(-1.0, m(inv(d2J_dx2), d2J_dxdy1));
	val edx_dy2 = sc(-1.0, m(inv(d2J_dx2), d2J_dxdy2));

	val ecov0_x = sum(m(edx_dy1,tr(edx_dy1)),m(edx_dy2,tr(edx_dy2)) );

	/* With the egsl_promote we save the matrix in the previous
	   context */
	*cov0_x = egsl_promote(ecov0_x);
	*dx_dy1 = egsl_promote(edx_dy1);
	*dx_dy2 = egsl_promote(edx_dy2);

	egsl_pop_named("compute_covariance_exact");
	/* now edx_dy1 is not valid anymore, but *dx_dy1 is. */
}

val compute_C_k(val p_j1, val p_j2)  {
	val d = sub(p_j1, p_j2);
	double alpha = M_PI/2 + atan2( atv(d,1), atv(d,0));
	double c = cos(alpha); double s = sin(alpha);
	double m[2*2] = {
		c*c, c*s,
		c*s, s*s
	};
	return egsl_vFda(2,2,m);
}

val dC_drho(val p1, val p2) {
	double eps = 0.001;

	val C_k = compute_C_k(p1, p2);
	val p1b = sum(p1, sc(eps/egsl_norm(p1),p1));
	val C_k_eps = compute_C_k(p1b,p2);
	return sc(1/eps, sub(C_k_eps,C_k));
}


text:
/**
 */
#ifndef _CROSSDISP_UTILS_H
#define _CROSSDISP_UTILS_H

#include "spc_trace_functions.h"
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_statistics.h>
#include "aXe_grism.h"
#include "aXe_utils.h"
#include <math.h>
#include "trace_conf.h"

/**
 * Structure: drz_pars
 *  A description of an ellipse with a,b, theta.
 */
typedef struct
{
  gsl_matrix *coeffs;
  d_point     offset;
  px_point    npixels;
}
drz_pars;

int
drizzle_distort(const gsl_vector * x, void *params, gsl_vector * f);

d_point
undistort_point(gsl_matrix *coeffs, const px_point pixmax,
                  d_point xy_image);

d_point
distort_point(gsl_matrix *coeffs, const px_point pixmax, d_point xy_image);

extern gsl_matrix *
get_crossdisp_matrix(char * filename, int sci_numext);

extern d_point
get_axis_scales(beam actbeam, gsl_matrix * drzcoeffs, px_point pixmax);

extern double
get_crossdisp_scale(trace_func  *trace, d_point refpnt, gsl_matrix * drzcoeffs, px_point pixmax);

extern d_point
get_drz_position(d_point in_point, gsl_matrix * drzcoeffs, px_point pixmax);

extern d_point
get_drz_position_free(d_point in_point, gsl_matrix * drzcoeffs, px_point pix_in, px_point pix_out);

extern trace_func *
get_tracefunc_at(char *filename, d_point p);

extern double
evaln(double x, double y, gsl_matrix * drzcoeffs, int row);

extern double
devalndx(double x, double y, gsl_matrix * drzcoeffs, int row);

extern double
devalndy(double x, double y, gsl_matrix * drzcoeffs, int row);

extern gsl_matrix *
get_jacobian(int i, int j, gsl_matrix * drzcoeffs, int width, int height);

extern double
get_det_jacobian(int i, int j, gsl_matrix * drzcoeffs, int width, int height);

#endif

max_stars_repo_name: sosey/pyaxe

max_issues_repo_name: sosey/pyaxe

max_forks_repo_name: sosey/pyaxe