1 files changed, 51 insertions, 0 deletions
diff --git a/src/gaussian-nd-random.c b/src/gaussian-nd-random.c
new file mode 100644
index 0000000..16abc94
--- /dev/null
+++ b/src/gaussian-nd-random.c
@@ -0,0 +1,51 @@
+#include <gsl/gsl_blas.h>
+#include <gsl/gsl_randist.h>
+#include <gsl/gsl_sf_gamma.h>
+#include "nd-random.h"
+#include "utils.h"
+
+#define INTEGRATION_EPSABS 1e-3
+#define INTEGRATION_EPSREL 1e-3
+
+unsigned int shifted_gaussian_random_vector
+(const gsl_rng* r, const gsl_vector* mean, double theta_max, double standard_deviation, gsl_vector* x)
+{
+  for (int i=0; i<x->size; i++)
+    gsl_vector_set(x, i, gsl_ran_gaussian(r, standard_deviation));
+  gsl_vector_add(x, mean);
+  gsl_blas_dscal(1/gsl_blas_dnrm2(x), x);
+  if (angle_between_vectors(x, mean) > theta_max)
+    return 1 + shifted_gaussian_random_vector(r, mean, theta_max, standard_deviation, x);
+  else return 1;
+}
+
+double shifted_gaussian_pdf (double theta, double mean, double theta_max,
+                             double standard_deviation, unsigned int dimension, gsl_integration_workspace* w)
+{
+  // NOTE: Due to the rejection sampling, the returned result is only
+  // proportional to the actual PDF.
+  if (theta > theta_max) return 0;
+  
+  double projection = mean*cos(theta);
+  double integrand (double r, void* params)
+  {
+    return gsl_pow_uint(r, dimension - 1)
+      * gaussian_pdf((r - projection) / standard_deviation);
+  }
+  
+  gsl_function gsl_integrand = {&integrand};
+  double result, error;
+  /* gsl_integration_qagiu(&gsl_integrand, 0, INTEGRATION_EPSABS, INTEGRATION_EPSREL, */
+  /*                       w->limit, w, &result, &error); */
+  double rmax = 0.5 * (projection + sqrt(gsl_pow_2(projection) + 4*(dimension-1)*gsl_pow_2(standard_deviation)));
+  double halfwidth = 3 * standard_deviation;
+  gsl_integration_qag(&gsl_integrand,
+                      rmax - halfwidth < 0 ? 0 : rmax - halfwidth,
+                      rmax + halfwidth,
+                      INTEGRATION_EPSABS, INTEGRATION_EPSREL,
+                      w->limit, GSL_INTEG_GAUSS41, w, &result, &error);
+  return result
+    * gaussian_pdf(sqrt(gsl_pow_2(mean) - gsl_pow_2(projection)) / standard_deviation)
+    / gsl_pow_uint(M_SQRT2*sqrt(M_PI), dimension - 2)
+    / gsl_pow_uint(standard_deviation, dimension);
+}