text:
//
//  gemm.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/30/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _gemm_h
#define _gemm_h

/// @file gemm.h Computes general matrix-matrix products.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Computes products of real matrices.
   ///
   /// For real matrices A,B,C and real scalars alpha, beta
   ///
   ///        C := alpha * op(A) * op(B) + beta * C
   ///
   /// is calculated, where op(X) = X or X'.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param transA Specifies whether op(A)=A or A' as follows:
   ///
   ///        if transA = 'N' or 'n' then op(A)=A
   ///        if transA = 'T' or 't' then op(A)=A'
   ///        if transA = 'C' or 'c' then op(A)=A'
   ///
   /// @param transB Specifies whether op(B)=B or B' as follows:
   ///
   ///        if transB = 'N' or 'n' then op(B)=B
   ///        if transB = 'T' or 't' then op(B)=B'
   ///        if transB = 'C' or 'c' then op(B)=B'
   ///
   /// @param m The number of rows of the matrices C and op(A).  m>=0
   /// @param n The number of columns of the matrices C and op(B).  n>=0
   /// @param k The number of columns of the matrix op(A), and the number of rows of the matrix op(B).  k>=0
   /// @param alpha Real scalar.
   /// @param A Pointer to real matrix A, where op(A) is m-by-k.
   /// @param ldA Column length of the matrix A.  If transA='N' or 'n', ldA>=m; otherwise, ldA>=k.
   /// @param B Pointer to real matrix B, where op(B) is k-by-n.
   /// @param ldB Column length of the matrix B.  If transB='N' or 'n', ldB>=k; otherwise, ldA>=n.
   /// @param beta Real scalar.
   /// @param C Pointer to real matrix C, where C is m-by-n.
   /// @param ldC Column length of the matrix C.  ldC>=m.
   /// @ingroup BLAS

   template<typename real_t>
   int GEMM(char transA, char transB, int_t m, int_t n, int_t k, real_t alpha, real_t *A, int_t ldA, real_t *B, int_t ldB, real_t beta, real_t *C, int_t ldC)
   {
      using std::toupper;
      const real_t zero(0.0);
      const real_t one(1.0);
      int_t i,j,l;
      real_t s,t;
      real_t *a,*b,*c;

      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;
      else if((m==0)||(n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='N'))
      {
         c=C;
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            for(l=0;l<k;l++)
            {
               for(i=0;i<m;i++)
               {
                  c[i]+=alpha*b[l]*a[i];
               }
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA!='N')&&(transB=='N'))
      {
         b=B;
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[l];
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB!='N'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            b=B;
            for(l=0;l<k;l++)
            {
               t=alpha*b[j];
               for(i=0;i<m;i++)
               {
                  c[i]+=t*a[i];
               }
               a+=ldA;
               b+=ldB;
            }
            c+=ldC;
         }
      }
      else
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[j];
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }

      }
      return 0;
   }

   /// @brief Computes products of complex matrices.
   ///
   /// For complex matrices A,B,C and complex scalars alpha, beta
   ///
   ///        C := alpha * op(A) * op(B) + beta * C
   ///
   /// is calculated, where op(X) = X, X' or X.'.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param transA Specifies whether op(A)=A, A.' or A' as follows:
   ///
   ///        if transA = 'N' or 'n' then op(A)=A
   ///        if transA = 'T' or 't' then op(A)=A.'
   ///        if transA = 'C' or 'c' then op(A)=A'
   ///
   /// @param transB Specifies whether op(B)=B, B.' or B' as follows:
   ///
   ///        if transB = 'N' or 'n' then op(B)=B
   ///        if transB = 'T' or 't' then op(B)=B.'
   ///        if transB = 'C' or 'c' then op(B)=B'
   ///
   /// @param m The number of rows of the matrices C and op(A).  m>=0
   /// @param n The number of columns of the matrices C and op(B).  n>=0
   /// @param k The number of columns of the matrix op(A), and the number of rows of the matrix op(B).  k>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex matrix A, where op(A) is m-by-k.
   /// @param ldA Column length of the matrix A.  If transA='N' or 'n', ldA>=m; otherwise, ldA>=k.
   /// @param B Pointer to complex matrix B, where op(B) is k-by-n.
   /// @param ldB Column length of the matrix B.  If transB='N' or 'n', ldB>=k; otherwise, ldA>=n.
   /// @param beta Complex scalar.
   /// @param C Pointer to complex matrix C, where C is m-by-n.
   /// @param ldC Column length of the matrix C.  ldC>=m.
   /// @ingroup BLAS

   template<typename real_t>
   int GEMM(char transA, char transB, int_t m, int_t n, int_t k, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB, complex<real_t> beta, complex<real_t> *C, int_t ldC)
   {
      using std::conj;
      using std::toupper;
      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,l;
      complex<real_t> *a,*b,*c;
      complex<real_t> s,t;

      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;
      else if((m==0)||(n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='N'))
      {
         c=C;
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            for(l=0;l<k;l++)
            {
               for(i=0;i<m;i++)
               {
                  c[i]+=alpha*b[l]*a[i];
               }
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='T')&&(transB=='N'))
      {
         b=B;
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[l];
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='C')&&(transB=='N'))
      {
         b=B;
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               for(l=0;l<k;l++)
               {
                  s+=conj(a[l])*b[l];
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            b+=ldB;
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='T'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            b=B;
            for(l=0;l<k;l++)
            {
               t=alpha*b[j];
               for(i=0;i<m;i++)
               {
                  c[i]+=t*a[i];
               }
               a+=ldA;
               b+=ldB;
            }
            c+=ldC;
         }
      }
      else if((transA=='N')&&(transB=='C'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            a=A;
            b=B;
            for(l=0;l<k;l++)
            {
               t=alpha*conj(b[j]);
               for(i=0;i<m;i++)
               {
                  c[i]+=t*a[i];
               }
               a+=ldA;
               b+=ldB;
            }
            c+=ldC;
         }
      }
      else if((transA=='T')&&(transB=='T'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*b[j];
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      else if((transA=='C')&&(transB=='T'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=conj(a[l])*b[j];
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      else if((transA=='T')&&(transB=='C'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=a[l]*conj(b[j]);
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      else if((transA=='C')&&(transB=='C'))
      {
         c=C;
         for(j=0;j<n;j++)
         {
            a=A;
            for(i=0;i<m;i++)
            {
               s=zero;
               b=B;
               for(l=0;l<k;l++)
               {
                  s+=conj(a[l])*conj(b[j]);
                  b+=ldB;
               }
               c[i]=alpha*s+beta*c[i];
               a+=ldA;
            }
            c+=ldC;
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template<> int GEMM<float>(char transA, char transB, int_t m, int_t n, int_t k, float alpha, float *A, int_t ldA, float *B, int_t ldB, float beta, float *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_sgemm(CblasColMajor,TransA,TransB,m,n,k,alpha,A,ldA,B,ldB,beta,C,ldC);

      return 0;
   }

   template<> int GEMM<double>(char transA, char transB, int_t m, int_t n, int_t k, double alpha, double *A, int_t ldA, double *B, int_t ldB, double beta, double *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_dgemm(CblasColMajor,TransA,TransB,m,n,k,alpha,A,ldA,B,ldB,beta,C,ldC);

      return 0;
   }

   template<> int GEMM<float>(char transA, char transB, int_t m, int_t n, int_t k, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *B, int_t ldB, complex<float> beta, complex<float> *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_cgemm(CblasColMajor,TransA,TransB,m,n,k,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

   template<> int GEMM<double>(char transA, char transB, int_t m, int_t n, int_t k, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *B, int_t ldB, complex<double> beta, complex<double> *C, int_t ldC)
   {
      transA=toupper(transA);
      transB=toupper(transB);

      if((transA!='N')&&(transA!='T')&&(transA!='C'))
         return -1;
      else if((transB!='N')&&(transB!='T')&&(transB!='C'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(k<0)
         return -5;
      else if(ldA<((transA!='N')?k:m))
         return -8;
      else if(ldB<((transB!='N')?n:k))
         return -10;
      else if(ldC<m)
         return -13;

      const CBLAS_TRANSPOSE TransA=(transA=='N')?CblasNoTrans:((transA=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_TRANSPOSE TransB=(transB=='N')?CblasNoTrans:((transB=='T')?CblasTrans:CblasConjTrans);

      cblas_zgemm(CblasColMajor,TransA,TransB,m,n,k,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

#endif
}
#endif

max_stars_repo_name: langou/latl

max_issues_repo_name: langou/latl

max_forks_repo_name: langou/latl


text:
/* monte/miser.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Michael Booth
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* MISER.  Based on the algorithm described in the following article,

   W.H. Press, G.R. Farrar, "Recursive Stratified Sampling for
   Multidimensional Monte Carlo Integration", Computers in Physics,
   v4 (1990), pp190-195.

*/

/* Author: MJB */
/* Modified by Brian Gough 12/2000 */

#include <config.h>
#include <math.h>
#include <stdlib.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_miser.h>

static int
estimate_corrmc (gsl_monte_function * f,
		 const double xl[], const double xu[],
		 size_t dim, size_t calls,
		 gsl_rng * r,
		 gsl_monte_miser_state * state,
		 double *result, double *abserr,
		 const double xmid[], double sigma_l[], double sigma_r[]);

int
gsl_monte_miser_integrate (gsl_monte_function * f,
			   const double xl[], const double xu[],
			   size_t dim, size_t calls,
			   gsl_rng * r,
			   gsl_monte_miser_state * state,
			   double *result, double *abserr)
{
  size_t n, estimate_calls, calls_l, calls_r;
  const size_t min_calls = state->min_calls;
  size_t i;
  size_t i_bisect;
  int found_best;

  double res_est = 0, err_est = 0;
  double res_r = 0, err_r = 0, res_l = 0, err_l = 0;
  double xbi_l, xbi_m, xbi_r, s;

  double vol;
  double weight_l, weight_r;

  double *x = state->x;
  double *xmid = state->xmid;
  double *sigma_l = state->sigma_l, *sigma_r = state->sigma_r;

  if (dim != state->dim)
    {
      GSL_ERROR ("number of dimensions must match allocated size", GSL_EINVAL);
    }

  for (i = 0; i < dim; i++)
    {
      if (xu[i] <= xl[i])
	{
	  GSL_ERROR ("xu must be greater than xl", GSL_EINVAL);
	}

      if (xu[i] - xl[i] > GSL_DBL_MAX)
	{
	  GSL_ERROR ("Range of integration is too large, please rescale",
		     GSL_EINVAL);
	}
    }

  if (state->alpha < 0)
    {
      GSL_ERROR ("alpha must be non-negative", GSL_EINVAL);
    }

  /* Compute volume */

  vol = 1;

  for (i = 0; i < dim; i++)
    {
      vol *= xu[i] - xl[i];
    }

  if (calls < state->min_calls_per_bisection)
    {
      double m = 0.0, q = 0.0;

      if (calls < 2)
	{
	  GSL_ERROR ("insufficient calls for subvolume", GSL_EFAILED);
	}

      for (n = 0; n < calls; n++)
	{
	  /* Choose a random point in the integration region */

	  for (i = 0; i < dim; i++)
	    {
	      x[i] = xl[i] + gsl_rng_uniform_pos (r) * (xu[i] - xl[i]);
	    }

	  {
	    double fval = GSL_MONTE_FN_EVAL (f, x);

	    /* recurrence for mean and variance */

	    double d = fval - m;
	    m += d / (n + 1.0);
	    q += d * d * (n / (n + 1.0));
	  }
	}

      *result = vol * m;

      *abserr = vol * sqrt (q / (calls * (calls - 1.0)));

      return GSL_SUCCESS;
    }

  estimate_calls = GSL_MAX (min_calls, calls * (state->estimate_frac));

  if (estimate_calls < 4 * dim)
    {
      GSL_ERROR ("insufficient calls to sample all halfspaces", GSL_ESANITY);
    }

  /* Flip coins to bisect the integration region with some fuzz */

  for (i = 0; i < dim; i++)
    {
      s = (gsl_rng_uniform (r) - 0.5) >= 0.0 ? state->dither : -state->dither;
      state->xmid[i] = (0.5 + s) * xl[i] + (0.5 - s) * xu[i];
    }

  /* The idea is to chose the direction to bisect based on which will
     give the smallest total variance.  We could (and may do so later)
     use MC to compute these variances.  But the NR guys simply estimate
     the variances by finding the min and max function values
     for each half-region for each bisection. */

  estimate_corrmc (f, xl, xu, dim, estimate_calls,
		   r, state, &res_est, &err_est, xmid, sigma_l, sigma_r);

  /* We have now used up some calls for the estimation */

  calls -= estimate_calls;

  /* Now find direction with the smallest total "variance" */

  {
    double best_var = GSL_DBL_MAX;
    double beta = 2.0 / (1.0 + state->alpha);
    found_best = 0;
    i_bisect = 0;
    weight_l = weight_r = 1.0;

    for (i = 0; i < dim; i++)
      {
	if (sigma_l[i] >= 0 && sigma_r[i] >= 0)
	  {
	    /* estimates are okay */
	    double var = pow (sigma_l[i], beta) + pow (sigma_r[i], beta);

	    if (var <= best_var)
	      {
		found_best = 1;
		best_var = var;
		i_bisect = i;
		weight_l = pow (sigma_l[i], beta);
		weight_r = pow (sigma_r[i], beta);
	      }
	  }
	else
	  {
	    if (sigma_l[i] < 0)
	      {
		GSL_ERROR ("no points in left-half space!", GSL_ESANITY);
	      }
	    if (sigma_r[i] < 0)
	      {
		GSL_ERROR ("no points in right-half space!", GSL_ESANITY);
	      }
	  }
      }
  }

  if (!found_best)
    {
      /* All estimates were the same, so chose a direction at random */

      i_bisect = gsl_rng_uniform_int (r, dim);
    }

  xbi_l = xl[i_bisect];
  xbi_m = xmid[i_bisect];
  xbi_r = xu[i_bisect];

  /* Get the actual fractional sizes of the two "halves", and
     distribute the remaining calls among them */

  {
    double fraction_l = fabs ((xbi_m - xbi_l) / (xbi_r - xbi_l));
    double fraction_r = 1 - fraction_l;

    double a = fraction_l * weight_l;
    double b = fraction_r * weight_r;

    calls_l = min_calls + (calls - 2 * min_calls) * a / (a + b);
    calls_r = min_calls + (calls - 2 * min_calls) * b / (a + b);
  }

  /* Compute the integral for the left hand side of the bisection */

  /* Due to the recursive nature of the algorithm we must allocate
     some new memory for each recursive call */

  {
    int status;

    double *xu_tmp = (double *) malloc (dim * sizeof (double));

    if (xu_tmp == 0)
      {
	GSL_ERROR_VAL ("out of memory for left workspace", GSL_ENOMEM, 0);
      }

    for (i = 0; i < dim; i++)
      {
	xu_tmp[i] = xu[i];
      }

    xu_tmp[i_bisect] = xbi_m;

    status = gsl_monte_miser_integrate (f, xl, xu_tmp,
					dim, calls_l, r, state,
					&res_l, &err_l);
    free (xu_tmp);

    if (status != GSL_SUCCESS)
      {
	return status;
      }
  }

  /* Compute the integral for the right hand side of the bisection */

  {
    int status;

    double *xl_tmp = (double *) malloc (dim * sizeof (double));

    if (xl_tmp == 0)
      {
	GSL_ERROR_VAL ("out of memory for right workspace", GSL_ENOMEM, 0);
      }

    for (i = 0; i < dim; i++)
      {
	xl_tmp[i] = xl[i];
      }

    xl_tmp[i_bisect] = xbi_m;

    status = gsl_monte_miser_integrate (f, xl_tmp, xu,
					dim, calls_r, r, state,
					&res_r, &err_r);
    free (xl_tmp);

    if (status != GSL_SUCCESS)
      {
	return status;
      }
  }

  *result = res_l + res_r;
  *abserr = sqrt (err_l * err_l + err_r * err_r);

  return GSL_SUCCESS;
}

gsl_monte_miser_state *
gsl_monte_miser_alloc (size_t dim)
{
  gsl_monte_miser_state *s =
    (gsl_monte_miser_state *) malloc (sizeof (gsl_monte_miser_state));

  if (s == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for miser state struct",
		     GSL_ENOMEM, 0);
    }

  s->x = (double *) malloc (dim * sizeof (double));

  if (s->x == 0)
    {
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
    }

  s->xmid = (double *) malloc (dim * sizeof (double));

  if (s->xmid == 0)
    {
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for xmid", GSL_ENOMEM, 0);
    }

  s->sigma_l = (double *) malloc (dim * sizeof (double));

  if (s->sigma_l == 0)
    {
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for sigma_l", GSL_ENOMEM, 0);
    }

  s->sigma_r = (double *) malloc (dim * sizeof (double));

  if (s->sigma_r == 0)
    {
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for sigma_r", GSL_ENOMEM, 0);
    }

  s->fmax_l = (double *) malloc (dim * sizeof (double));

  if (s->fmax_l == 0)
    {
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmax_l", GSL_ENOMEM, 0);
    }

  s->fmax_r = (double *) malloc (dim * sizeof (double));

  if (s->fmax_r == 0)
    {
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmax_r", GSL_ENOMEM, 0);
    }

  s->fmin_l = (double *) malloc (dim * sizeof (double));

  if (s->fmin_l == 0)
    {
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmin_l", GSL_ENOMEM, 0);
    }

  s->fmin_r = (double *) malloc (dim * sizeof (double));

  if (s->fmin_r == 0)
    {
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmin_r", GSL_ENOMEM, 0);
    }

  s->fsum_l = (double *) malloc (dim * sizeof (double));

  if (s->fsum_l == 0)
    {
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum_l", GSL_ENOMEM, 0);
    }

  s->fsum_r = (double *) malloc (dim * sizeof (double));

  if (s->fsum_r == 0)
    {
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum_r", GSL_ENOMEM, 0);
    }

  s->fsum2_l = (double *) malloc (dim * sizeof (double));

  if (s->fsum2_l == 0)
    {
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_l", GSL_ENOMEM, 0);
    }

  s->fsum2_r = (double *) malloc (dim * sizeof (double));

  if (s->fsum2_r == 0)
    {
      free (s->fsum2_l);
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_r", GSL_ENOMEM, 0);
    }

  s->hits_r = (size_t *) malloc (dim * sizeof (size_t));

  if (s->hits_r == 0)
    {
      free (s->fsum2_r);
      free (s->fsum2_l);
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_r", GSL_ENOMEM, 0);
    }

  s->hits_l = (size_t *) malloc (dim * sizeof (size_t));

  if (s->hits_l == 0)
    {
      free (s->hits_r);
      free (s->fsum2_r);
      free (s->fsum2_l);
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_r", GSL_ENOMEM, 0);
    }

  s->dim = dim;

  gsl_monte_miser_init (s);

  return s;
}

int
gsl_monte_miser_init (gsl_monte_miser_state * s)
{
  /* We use 8 points in each halfspace to estimate the variance. There are
     2*dim halfspaces. A variance estimate requires a minimum of 2 points. */
  s->min_calls = 16 * s->dim;
  s->min_calls_per_bisection = 32 * s->min_calls;
  s->estimate_frac = 0.1;
  s->alpha = 2.0;
  s->dither = 0.0;

  return GSL_SUCCESS;
}

void
gsl_monte_miser_free (gsl_monte_miser_state * s)
{
  free (s->hits_r);
  free (s->hits_l);
  free (s->fsum2_r);
  free (s->fsum2_l);
  free (s->fsum_r);
  free (s->fsum_l);
  free (s->fmin_r);
  free (s->fmin_l);
  free (s->fmax_r);
  free (s->fmax_l);
  free (s->sigma_r);
  free (s->sigma_l);
  free (s->xmid);
  free (s->x);
  free (s);
}

static int
estimate_corrmc (gsl_monte_function * f,
		 const double xl[], const double xu[],
		 size_t dim, size_t calls,
		 gsl_rng * r,
		 gsl_monte_miser_state * state,
		 double *result, double *abserr,
		 const double xmid[], double sigma_l[], double sigma_r[])
{
  size_t i, n;

  double *x = state->x;
  double *fsum_l = state->fsum_l;
  double *fsum_r = state->fsum_r;
  double *fsum2_l = state->fsum2_l;
  double *fsum2_r = state->fsum2_r;
  size_t *hits_l = state->hits_l;
  size_t *hits_r = state->hits_r;

  double m = 0.0, q = 0.0;
  double vol = 1.0;

  for (i = 0; i < dim; i++)
    {
      vol *= xu[i] - xl[i];
      hits_l[i] = hits_r[i] = 0;
      fsum_l[i] = fsum_r[i] = 0.0;
      fsum2_l[i] = fsum2_r[i] = 0.0;
      sigma_l[i] = sigma_r[i] = -1;
    }

  for (n = 0; n < calls; n++)
    {
      double fval;

      unsigned int j = (n/2) % dim;
      unsigned int side = (n % 2);

      for (i = 0; i < dim; i++)
	{
          double z = gsl_rng_uniform_pos (r) ;

          if (i != j)
            {
              x[i] = xl[i] + z * (xu[i] - xl[i]);
            }
          else
            {
              if (side == 0)
                {
                  x[i] = xmid[i] + z * (xu[i] - xmid[i]);
                }
              else
                {
                  x[i] = xl[i] + z * (xmid[i] - xl[i]);
                }
            }
	}

      fval = GSL_MONTE_FN_EVAL (f, x);

      /* recurrence for mean and variance */
      {
	double d = fval - m;
	m += d / (n + 1.0);
	q += d * d * (n / (n + 1.0));
      }

      /* compute the variances on each side of the bisection */
      for (i = 0; i < dim; i++)
	{
	  if (x[i] <= xmid[i])
	    {
	      fsum_l[i] += fval;
	      fsum2_l[i] += fval * fval;
	      hits_l[i]++;
	    }
	  else
	    {
	      fsum_r[i] += fval;
	      fsum2_r[i] += fval * fval;
	      hits_r[i]++;
	    }
	}
    }

  for (i = 0; i < dim; i++)
    {
      double fraction_l = (xmid[i] - xl[i]) / (xu[i] - xl[i]);

      if (hits_l[i] > 0)
	{
	  fsum_l[i] /= hits_l[i];
	  sigma_l[i] = sqrt (fsum2_l[i] - fsum_l[i] * fsum_l[i] / hits_l[i]);
	  sigma_l[i] *= fraction_l * vol / hits_l[i];
	}

      if (hits_r[i] > 0)
	{
	  fsum_r[i] /= hits_r[i];
	  sigma_r[i] = sqrt (fsum2_r[i] - fsum_r[i] * fsum_r[i] / hits_r[i]);
	  sigma_r[i] *= (1 - fraction_l) * vol / hits_r[i];
	}
    }

  *result = vol * m;

  if (calls < 2)
    {
      *abserr = GSL_POSINF;
    }
  else
    {
      *abserr = vol * sqrt (q / (calls * (calls - 1.0)));
    }

  return GSL_SUCCESS;
}

ext: c


text:
/**
 * @file      MeshAdaptors.h
 *
 * @brief     This file contains mesh <-> Eigen matrix adaptors
 *
 * @author    Stefan Reinhold
 * @copyright Copyright (C) 2018 Stefan Reinhold  -- All Rights Reserved.
 *            You may use, distribute and modify this code under the terms of
 *            the AFL 3.0 license; see LICENSE for full license details.
 */

#pragma once

#include "Mesh.h"

#include <Eigen/Core>
#include <gsl/gsl>

namespace CortidQCT {
namespace Internal {
namespace Adaptor {

template <class T>
inline Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic> const>
vertexMap(Mesh<T> const &mesh) {
  return mesh.withUnsafeVertexPointer([&mesh](auto const *ptr) {
    return Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic> const>{
        ptr, 3, gsl::narrow_cast<Eigen::Index>(mesh.vertexCount())};
  });
}

template <class T>
inline Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic>>
vertexMap(Mesh<T> &mesh) {
  return mesh.withUnsafeVertexPointer([&mesh](auto *ptr) {
    return Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic>>{
        ptr, 3, gsl::narrow_cast<Eigen::Index>(mesh.vertexCount())};
  });
}

template <class T>
inline Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic> const>
vertexNormalMap(Mesh<T> const &mesh) {
  return mesh.withUnsafeVertexNormalPointer([&mesh](auto const *ptr) {
    return Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic> const>{
        ptr, 3, gsl::narrow_cast<Eigen::Index>(mesh.vertexCount())};
  });
}

template <class T>
inline Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic>>
vertexNormalMap(Mesh<T> &mesh) {
  return mesh.withUnsafeVertexNormalPointer([&mesh](auto *ptr) {
    return Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic>>{
        ptr, 3, gsl::narrow_cast<Eigen::Index>(mesh.vertexCount())};
  });
}

template <class T>
inline Eigen::Map<
    Eigen::Matrix<typename Mesh<T>::Index, 3, Eigen::Dynamic> const>
indexMap(Mesh<T> const &mesh) {
  using S = typename Mesh<T>::Index;
  return mesh.withUnsafeIndexPointer([&mesh](auto const *ptr) {
    return Eigen::Map<Eigen::Matrix<S, 3, Eigen::Dynamic> const>{
        ptr, 3, gsl::narrow_cast<Eigen::Index>(mesh.triangleCount())};
  });
}

template <class T>
inline Eigen::Map<Eigen::Matrix<typename Mesh<T>::Index, 3, Eigen::Dynamic>>
indexMap(Mesh<T> &mesh) {
  using S = typename Mesh<T>::Index;
  return mesh.withUnsafeIndexPointer([&mesh](auto *ptr) {
    return Eigen::Map<Eigen::Matrix<S, 3, Eigen::Dynamic>>{
        ptr, 3, gsl::narrow_cast<Eigen::Index>(mesh.triangleCount())};
  });
}

template <class T>
inline Eigen::Map<
    Eigen::Matrix<typename Mesh<T>::Label, Eigen::Dynamic, 1> const>
labelMap(Mesh<T> const &mesh) {
  using S = typename Mesh<T>::Label;
  return mesh.withUnsafeLabelPointer([&mesh](auto const *ptr) {
    return Eigen::Map<Eigen::Matrix<S, Eigen::Dynamic, 1> const>{
        ptr, gsl::narrow_cast<Eigen::Index>(mesh.vertexCount())};
  });
}

template <class T>
inline Eigen::Map<Eigen::Matrix<typename Mesh<T>::Label, Eigen::Dynamic, 1>>
labelMap(Mesh<T> &mesh) {
  using S = typename Mesh<T>::Label;
  return mesh.withUnsafeLabelPointer([&mesh](auto *ptr) {
    return Eigen::Map<Eigen::Matrix<S, Eigen::Dynamic, 1>>{
        ptr, gsl::narrow_cast<Eigen::Index>(mesh.vertexCount())};
  });
}

} // namespace Adaptor
} // namespace Internal
} // namespace CortidQCT

ext: h

max_stars_repo_name: ithron/CortidQCT

max_issues_repo_name: ithron/CortidQCT

max_forks_repo_name: ithron/CortidQCT


text:
#ifndef VEC3_H
#define VEC3_H

#include <cmath>
#include <iostream>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
// you need to add the following libraries to your project : gsl, gslcblas

class Vec3 {
private:
    float mVals[3];
public:
    Vec3() {}
    Vec3( float x , float y , float z ) {
       mVals[0] = x; mVals[1] = y; mVals[2] = z;
    }
    float & operator [] (unsigned int c) { return mVals[c]; }
    float operator [] (unsigned int c) const { return mVals[c]; }
    void operator = (Vec3 const & other) {
       mVals[0] = other[0] ; mVals[1] = other[1]; mVals[2] = other[2];
    }
    float squareLength() const {
       return mVals[0]*mVals[0] + mVals[1]*mVals[1] + mVals[2]*mVals[2];
    }
    float length() const { return sqrt( squareLength() ); }
    void normalize() { float L = length(); mVals[0] /= L; mVals[1] /= L; mVals[2] /= L; }
    static float dot( Vec3 const & a , Vec3 const & b ) {
       return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
    }
    static Vec3 cross( Vec3 const & a , Vec3 const & b ) {
       return Vec3( a[1]*b[2] - a[2]*b[1] ,
                    a[2]*b[0] - a[0]*b[2] ,
                    a[0]*b[1] - a[1]*b[0] );
    }
    void operator += (Vec3 const & other) {
        mVals[0] += other[0];
        mVals[1] += other[1];
        mVals[2] += other[2];
    }
    void operator -= (Vec3 const & other) {
        mVals[0] -= other[0];
        mVals[1] -= other[1];
        mVals[2] -= other[2];
    }
    void operator *= (float s) {
        mVals[0] *= s;
        mVals[1] *= s;
        mVals[2] *= s;
    }
    void operator /= (float s) {
        mVals[0] /= s;
        mVals[1] /= s;
        mVals[2] /= s;
    }

    static Vec3 Rand(float magnitude = 1.f) {
        return Vec3( magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) , magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) , magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) );
    }

    Vec3 getOrthogonal() const {
        // CAREFUL ! THE NORM IS NOT PRESERVED !!!!
        if( mVals[0] == 0 ) {
            return Vec3( 0 , mVals[2] , -mVals[1] );
        }
        else if( mVals[1] == 0 ) {
            return Vec3( mVals[2] , 0 , -mVals[0] );
        }
        return Vec3( mVals[1] , -mVals[0] , 0 );
    }
};

static inline Vec3 operator + (Vec3 const & a , Vec3 const & b) {
   return Vec3(a[0]+b[0] , a[1]+b[1] , a[2]+b[2]);
}
static inline Vec3 operator - (Vec3 const & a , Vec3 const & b) {
   return Vec3(a[0]-b[0] , a[1]-b[1] , a[2]-b[2]);
}
static inline Vec3 operator * (float a , Vec3 const & b) {
   return Vec3(a*b[0] , a*b[1] , a*b[2]);
}
static inline Vec3 operator / (Vec3 const &  a , float b) {
   return Vec3(a[0]/b , a[1]/b , a[2]/b);
}
static inline std::ostream & operator << (std::ostream & s , Vec3 const & p) {
    s << p[0] << " " << p[1] << " " << p[2];
    return s;
}
static inline std::istream & operator >> (std::istream & s , Vec3 & p) {
    s >> p[0] >> p[1] >> p[2];
    return s;
}

class Mat3
{
public:
    ////////////         CONSTRUCTORS          //////////////
    Mat3()
    {
        vals[0] = 0;
        vals[1] = 0;
        vals[2] = 0;
        vals[3] = 0;
        vals[4] = 0;
        vals[5] = 0;
        vals[6] = 0;
        vals[7] = 0;
        vals[8] = 0;
    }
    Mat3( float v1 , float v2 , float v3 , float v4 , float v5 , float v6 , float v7 , float v8 , float v9)
    {
        vals[0] = v1;
        vals[1] = v2;
        vals[2] = v3;
        vals[3] = v4;
        vals[4] = v5;
        vals[5] = v6;
        vals[6] = v7;
        vals[7] = v8;
        vals[8] = v9;
    }
    Mat3( const Mat3 & m )
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) = m(i,j);
    }

    bool isnan() const {
        return std::isnan(vals[0]) || std::isnan(vals[1]) || std::isnan(vals[2])
                 || std::isnan(vals[3]) || std::isnan(vals[4]) || std::isnan(vals[5])
                 || std::isnan(vals[6]) || std::isnan(vals[7]) || std::isnan(vals[8]);
    }

    void operator = (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) = m(i,j);
    }

    void operator += (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) += m(i,j);
    }
    void operator -= (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) -= m(i,j);
    }
    void operator /= (double s)
    {
        for( unsigned int c = 0 ; c < 9 ; ++c )
            vals[c] /= s;
    }

    Mat3 operator - (const Mat3 & m2)
    {
        return Mat3( (*this)(0,0)-m2(0,0) , (*this)(0,1)-m2(0,1) , (*this)(0,2)-m2(0,2) , (*this)(1,0)-m2(1,0) , (*this)(1,1)-m2(1,1) , (*this)(1,2)-m2(1,2) , (*this)(2,0)-m2(2,0) , (*this)(2,1)-m2(2,1) , (*this)(2,2)-m2(2,2) );
    }
    Mat3 operator + (const Mat3 & m2)
    {
        return Mat3( (*this)(0,0)+m2(0,0) , (*this)(0,1)+m2(0,1) , (*this)(0,2)+m2(0,2) , (*this)(1,0)+m2(1,0) , (*this)(1,1)+m2(1,1) , (*this)(1,2)+m2(1,2) , (*this)(2,0)+m2(2,0) , (*this)(2,1)+m2(2,1) , (*this)(2,2)+m2(2,2) );
    }

    Mat3 operator / (float s)
    {
        return Mat3( (*this)(0,0)/s , (*this)(0,1)/s , (*this)(0,2)/s , (*this)(1,0)/s , (*this)(1,1)/s , (*this)(1,2)/s , (*this)(2,0)/s , (*this)(2,1)/s , (*this)(2,2)/s );
    }
    Mat3 operator * (float s)
    {
        return Mat3( (*this)(0,0)*s , (*this)(0,1)*s , (*this)(0,2)*s , (*this)(1,0)*s , (*this)(1,1)*s , (*this)(1,2)*s , (*this)(2,0)*s , (*this)(2,1)*s , (*this)(2,2)*s );
    }

    Vec3 operator * (const Vec3 & p) // computes m.p
    {
        return Vec3(
                    (*this)(0,0)*p[0] + (*this)(0,1)*p[1] + (*this)(0,2)*p[2],
                    (*this)(1,0)*p[0] + (*this)(1,1)*p[1] + (*this)(1,2)*p[2],
                    (*this)(2,0)*p[0] + (*this)(2,1)*p[1] + (*this)(2,2)*p[2]);
    }

    Mat3 operator * (const Mat3 & m2)
    {
        return Mat3(
                    (*this)(0,0)*m2(0,0) + (*this)(0,1)*m2(1,0) + (*this)(0,2)*m2(2,0) ,
                    (*this)(0,0)*m2(0,1) + (*this)(0,1)*m2(1,1) + (*this)(0,2)*m2(2,1) ,
                    (*this)(0,0)*m2(0,2) + (*this)(0,1)*m2(1,2) + (*this)(0,2)*m2(2,2) ,
                    (*this)(1,0)*m2(0,0) + (*this)(1,1)*m2(1,0) + (*this)(1,2)*m2(2,0) ,
                    (*this)(1,0)*m2(0,1) + (*this)(1,1)*m2(1,1) + (*this)(1,2)*m2(2,1) ,
                    (*this)(1,0)*m2(0,2) + (*this)(1,1)*m2(1,2) + (*this)(1,2)*m2(2,2) ,
                    (*this)(2,0)*m2(0,0) + (*this)(2,1)*m2(1,0) + (*this)(2,2)*m2(2,0) ,
                    (*this)(2,0)*m2(0,1) + (*this)(2,1)*m2(1,1) + (*this)(2,2)*m2(2,1) ,
                    (*this)(2,0)*m2(0,2) + (*this)(2,1)*m2(1,2) + (*this)(2,2)*m2(2,2)
                    );
    }

    ////////        ACCESS TO COORDINATES      /////////
    float operator () (unsigned int i , unsigned int j) const
    { return vals[3*i + j]; }
    float & operator () (unsigned int i , unsigned int j)
    { return vals[3*i + j]; }

    ////////        BASICS       /////////
    inline float sqrnorm()
    {
        return vals[0]*vals[0] + vals[1]*vals[1] + vals[2]*vals[2]
                + vals[3]*vals[3] + vals[4]*vals[4] + vals[5]*vals[5]
                + vals[6]*vals[6] +  vals[7]*vals[7] + vals[8]*vals[8];
    }

    inline float norm()
    { return sqrt( sqrnorm() ); }

    inline float determinant() const
    {
        return vals[0] * ( vals[4] * vals[8] - vals[7] * vals[5] )
                - vals[1] * ( vals[3] * vals[8] - vals[6] * vals[5] )
                + vals[2] * ( vals[3] * vals[7] - vals[6] * vals[4] );
    }

    static
    Mat3 pseudoInverse( Mat3 const & m , bool & isRealInverse , double defaultValueForInverseSingularValue = 0.0 )
    {
        float det = m.determinant();
        if( fabs(det) != 0.0 )
        {
            isRealInverse = true;
            return Mat3( m(1,1)*m(2,2) - m(2,1)*m(1,2) , m(0,2)*m(2,1) - m(0,1)*m(2,2) , m(0,1)*m(1,2) - m(0,2)*m(1,1) ,
                             m(1,2)*m(2,0) - m(1,0)*m(2,2) , m(0,0)*m(2,2) - m(0,2)*m(2,0) , m(0,2)*m(1,0) - m(0,0)*m(1,2) ,
                             m(1,0)*m(2,1) - m(1,1)*m(2,0) , m(0,1)*m(2,0) - m(0,0)*m(2,1) , m(0,0)*m(1,1) - m(0,1)*m(1,0) ) / det ;
        }

        // otherwise:
        isRealInverse = false;
        Mat3 U ; float sx ; float sy ; float sz ; Mat3 Vt;
        m.SVD(U,sx,sy,sz,Vt);
        float sxInv = sx == 0.0 ? 1.0 / sx : defaultValueForInverseSingularValue;
        float syInv = sy == 0.0 ? 1.0 / sy : defaultValueForInverseSingularValue;
        float szInv = sz == 0.0 ? 1.0 / sz : defaultValueForInverseSingularValue;
        return Vt.getTranspose() * Mat3::diag(sxInv , syInv , szInv) * U.getTranspose();
    }

    inline float trace() const
    { return vals[0] + vals[4] + vals[8]; }

    ////////        TRANSPOSE       /////////
    inline
    void transpose()
    {
        float xy = vals[1] , xz = vals[2] , yz = vals[5];
        vals[1] = vals[3];
        vals[3] = xy;
        vals[2] = vals[6];
        vals[6] = xz;
        vals[5] = vals[7];
        vals[7] = yz;
    }
    Mat3 getTranspose() const
    {
        return Mat3(vals[0],vals[3],vals[6],vals[1],vals[4],vals[7],vals[2],vals[5],vals[8]);
    }

    // ---------- ROTATION <-> AXIS/ANGLE ---------- //
    template< class point_t >
    void getAxisAndAngleFromRotationMatrix( point_t & axis , float & angle )
    {
        angle = acos( (trace() - 1.f) / 2.f );
        axis[0] = vals[7] - vals[5];
        axis[1] = vals[2] - vals[6];
        axis[2] = vals[3] - vals[1];
        axis.normalize();
    }

    template< class point_t >
    inline static
    Mat3 getRotationMatrixFromAxisAndAngle( const point_t & axis , float angle )
    {
        Mat3 w = vectorial(axis);
        return Identity() +  w * std::sin(angle) +  w * w * ((1.0) - std::cos(angle));
    }

    // ---------- STATIC STANDARD MATRICES ---------- //
    inline static Mat3 Identity()
    {  return Mat3(1,0,0  ,  0,1,0  ,  0,0,1);  }

    inline static Mat3 Zero()
    {  return Mat3(0,0,0  ,  0,0,0  ,  0,0,0);  }

    template< typename T2 >
    inline static Mat3 diag( T2 x , T2 y ,T2 z )
    {  return Mat3(x,0,0  ,  0,y,0  ,  0,0,z);  }

    template< class point_t >
    inline static Mat3 getFromCols(const point_t & c1 , const point_t & c2 , const point_t & c3)
    {
        // 0 1 2
        // 3 4 5
        // 6 7 8
        return Mat3( c1[0] , c2[0] , c3[0] ,
                         c1[1] , c2[1] , c3[1] ,
                         c1[2] , c2[2] , c3[2] );
    }
    template< class point_t >
    inline static Mat3 getFromRows(const point_t & r1 , const point_t & r2 , const point_t & r3)
    {
        // 0 1 2
        // 3 4 5
        // 6 7 8
        return Mat3( r1[0] , r1[1] , r1[2] ,
                         r2[0] , r2[1] , r2[2] ,
                         r3[0] , r3[1] , r3[2] );
    }

    inline static Mat3 RandRotation()
    {
        Vec3 axis(-1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  );
        axis.normalize();
        float angle = 2.0 * M_PI * ((float)(rand()) / (float)( RAND_MAX )   - 0.5 );

        return Mat3::getRotationMatrixFromAxisAndAngle( axis , angle );
    }

    inline static Mat3 RandRotation( float maxAngle )
    {
        Vec3 axis(-1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  );
        axis.normalize();
        float angle =  maxAngle * ((float)(rand()) / (float)( RAND_MAX )   - 0.5 );

        return Mat3::getRotationMatrixFromAxisAndAngle( axis , angle );
    }

    inline static Mat3 RandRotation( Vec3 twistAxis , double maxTwist , double maxRotation )
    {
        twistAxis.normalize();
        Vec3 u = twistAxis.getOrthogonal();
        u.normalize();
        const Vec3 & v = Vec3::cross(u,twistAxis);

        double uv_axis_angle = 2.0*M_PI * (double)(rand()) / (double)(RAND_MAX);
        const Vec3 & uv = cos(uv_axis_angle)*u + sin(uv_axis_angle)*v;

        double rotation_angle = maxRotation * ( ( 2.0 *(double)(rand()) / (double)(RAND_MAX) ) - 1.0 );
        double twist_angle = maxTwist * ( ( 2.0 *(double)(rand()) / (double)(RAND_MAX) ) - 1.0 );

        return Mat3::getRotationMatrixFromAxisAndAngle(uv , rotation_angle) * Mat3::getRotationMatrixFromAxisAndAngle(twistAxis , twist_angle);
    }

    void SVD( Mat3 & U , float & sx , float & sy , float & sz , Mat3 & Vt ) const
    {
        gsl_matrix * u = gsl_matrix_alloc(3,3);
        for(unsigned int i = 0 ; i < 3; ++i)
            for(unsigned int j = 0 ; j < 3; ++j)
                gsl_matrix_set( u , i , j , (*this)(i,j) );

        gsl_matrix * v = gsl_matrix_alloc(3,3);
        gsl_vector * s = gsl_vector_alloc(3);
        gsl_vector * work = gsl_vector_alloc(3);

        gsl_linalg_SV_decomp (u,
                              v,
                              s,
                              work);

        sx = s->data[0];
        sy = s->data[1];
        sz = s->data[2];
        for(unsigned int i = 0 ; i < 3; ++i)
        {
            for(unsigned int j = 0 ; j < 3; ++j)
            {
                U(i,j) = gsl_matrix_get( u , i , j );
                Vt(i,j) = gsl_matrix_get( v , j , i );
            }
        }

        gsl_matrix_free(u);
        gsl_matrix_free(v);
        gsl_vector_free(s);
        gsl_vector_free(work);

        // a transformation float is given as R.B.S.Bt, R = rotation , B = local basis (rotation matrix), S = scales in the basis B
        // it can be obtained from the svd decomposition of float = U Sigma Vt :
        // B = V
        // S = Sigma
        // R = U.Vt
    }

    // ---------- Projections onto Rotations ----------- //

    void setRotation()
    {
        Mat3 U,Vt;
        float sx,sy,sz;
        SVD(U,sx,sy,sz,Vt);
        const Mat3 & res = U*Vt;
        if( res.determinant() < 0 )
        {
            U(0,2) = -U(0,2);
            U(1,2) = -U(1,2);
            U(2,2) = -U(2,2);
            *this = (U*Vt);
            return;
        }
        // else
        *this = (res);
    }

    template< class point_t >
    inline static
    Mat3 tensor( const point_t & p1 , const point_t & p2 )
    {
        return Mat3(
                    p1[0]*p2[0] , p1[0]*p2[1] , p1[0]*p2[2],
                    p1[1]*p2[0] , p1[1]*p2[1] , p1[1]*p2[2],
                    p1[2]*p2[0] , p1[2]*p2[1] , p1[2]*p2[2]);
    }

    template< class point_t >
    inline static
    Mat3 vectorial( const point_t & p )
    {
        return Mat3(
                    0      , -p[2]  , p[1]     ,
                    p[2]   , 0      , - p[0]   ,
                    - p[1] , p[0]   , 0
                    );
    }

    Mat3 operator - () const
    {
        return Mat3( - vals[0],- vals[1],- vals[2],- vals[3],- vals[4],- vals[5],- vals[6],- vals[7],- vals[8] );
    }

private:
    float vals[9];
    // will be noted as :
    // 0 1 2
    // 3 4 5
    // 6 7 8
};

inline static
Mat3 operator * (float s , const Mat3 & m)
{
    return Mat3( m(0,0)*s , m(0,1)*s , m(0,2)*s , m(1,0)*s , m(1,1)*s , m(1,2)*s , m(2,0)*s , m(2,1)*s , m(2,2)*s );
}

inline static std::ostream & operator << (std::ostream & s , Mat3 const & m)
{
    s << m(0,0) << " \t" << m(0,1) << " \t" << m(0,2) << std::endl << m(1,0) << " \t" << m(1,1) << " \t" << m(1,2) << std::endl << m(2,0) << " \t" << m(2,1) << " \t" << m(2,2) << std::endl;
    return s;
}

#endif