1 files changed, 72 insertions, 0 deletions
diff --git a/src/gaussian-nd-random.sc b/src/gaussian-nd-random.sc
new file mode 100644
index 0000000..bfde566
--- /dev/null
+++ b/src/gaussian-nd-random.sc
@@ -0,0 +1,72 @@
+(sc-include "macros/macros")
+
+(pre-include "gsl/gsl_blas.h")
+(pre-include "gsl/gsl_integration.h")
+(pre-include "gsl/gsl_randist.h")
+(pre-include "utils.h")
+
+(pre-define INTEGRATION-EPSABS 1e-3)
+(pre-define INTEGRATION-EPSREL 1e-3)
+
+(define (shifted-gaussian-random-vector r mean theta-max standard-deviation x)
+  ((unsigned int) (const gsl-rng*) (const gsl-vector*) double double gsl-vector*)
+  "Return a random vector generated by the shifted Gaussian method."
+  (for-i i (: x size)
+         (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)
+  (cond
+   ((> (angle-between-vectors x mean) theta-max)
+    (return (+ 1 (shifted-gaussian-random-vector r mean theta-max standard-deviation x))))
+   (else (return 1))))
+
+(define (shifted-gaussian-pdf theta mean theta-max standard-deviation dimension w)
+  (double double double double double (unsigned int) gsl-integration-workspace*)
+  "Return the unnormalized probability density function at THETA for a
+random vector generated using the shifted Gaussian method."
+  (cond
+   ((> theta theta-max) (return 0))
+   (else
+    (define projection double (* mean (cos theta)))
+    (define (integrand r params) (double double void*)
+      (return (* (gsl-pow-uint r (- dimension 1))
+                 (gaussian-pdf (/ (- r projection)
+                                  standard-deviation)))))
+
+    (declare gsl-integrand (struct-variable gsl-function (function (address-of integrand)))
+             result double
+             error double)
+    
+    ;; (gsl-integration-qagiu (address-of gsl-integrand)
+    ;;                        0
+    ;;                        INTEGRATION-EPSABS
+    ;;                        INTEGRATION-EPSREL
+    ;;                        (: w limit)
+    ;;                        w
+    ;;                        (address-of result)
+    ;;                        (address-of error))
+    (let* ((rmax double (/ (+ projection
+                              (sqrt (+ (gsl-pow-2 projection)
+                                       (* 4
+                                          (- dimension 1)
+                                          (gsl-pow-2 standard-deviation)))))
+                           2))
+           (half-width double (* 3 standard-deviation)))
+      (gsl-integration-qag (address-of gsl-integrand)
+                           (if* (< (- rmax half-width) 0)
+                                0
+                                (- rmax half-width))
+                           (+ rmax half-width)
+                           INTEGRATION-EPSABS
+                           INTEGRATION-EPSREL
+                           (: w limit)
+                           GSL-INTEG-GAUSS41
+                           w
+                           (address-of result)
+                           (address-of error)))
+    (return (/ (* result
+                  (gaussian-pdf (/ (sqrt (- (gsl-pow-2 mean)
+                                            (gsl-pow-2 projection)))
+                                   standard-deviation)))
+               (gsl-pow-uint (sqrt (* 2 M-PI)) (- dimension 2))
+               (gsl-pow-uint standard-deviation dimension))))))