1 files changed, 101 insertions, 0 deletions
diff --git a/src/nd-random.c b/src/nd-random.c
new file mode 100644
index 0000000..25abc7a
--- /dev/null
+++ b/src/nd-random.c
@@ -0,0 +1,101 @@
+#include <float.h>
+#include <math.h>
+#include <gsl/gsl_blas.h>
+#include <gsl/gsl_randist.h>
+#include <gsl/gsl_sf_gamma.h>
+#include "nd-random.h"
+#include "utils.h"
+
+static double beta_inc_unnormalized (double a, double b, double x);
+static double incomplete_wallis_integral (double theta, unsigned int m);
+
+void random_direction_vector (const gsl_rng* r, gsl_vector* x)
+{
+  gsl_ran_dir_nd(r, x->size, x->data);
+}
+
+static void rotate_from_nth_canonical (gsl_vector* x, const gsl_vector* orient)
+{
+  const size_t n = x->size;
+  double xn = gsl_vector_get(x, n - 1);
+  double mun = gsl_vector_get(orient, n - 1);
+  gsl_vector_const_view orient_sub = gsl_vector_const_subvector(orient, 0, n - 1);
+  double b = gsl_blas_dnrm2(&orient_sub.vector);
+  double a = (dot_product(orient, x) - xn*mun) / b;
+  double c = mun, s = sqrt(1 - gsl_pow_2(c));
+  gsl_blas_daxpy((xn*s + a*(c - 1))/b, orient, x);
+  gsl_vector_set(x, n - 1, gsl_vector_get(x, n - 1)
+                 + xn*(c - 1) - a*s
+                 - mun*(xn*s + a*(c - 1))/b);
+}
+
+/* TODO: There is an edge case when mean is the (n-1)th canonical
+   basis vector. Fix it.
+*/
+void hollow_cone_random_vector (const gsl_rng* r, const gsl_vector* mean, double theta_min, double theta_max, gsl_vector* x)
+{
+  unsigned int n = x->size;
+  gsl_ran_dir_nd(r, n - 1, x->data);
+
+  // Generate random vector around the nth canonical basis vector
+  double omega_min = planar_angle_to_solid_angle(theta_min, n);
+  double omega_max = planar_angle_to_solid_angle(theta_max, n);
+  gsl_vector_scale(x, cos(theta_max) * tan(solid_angle_to_planar_angle(gsl_ran_flat(r, omega_min, omega_max), n)));
+  gsl_vector_set(x, n - 1, cos(theta_max));
+  gsl_vector_scale(x, 1.0/gsl_blas_dnrm2(x));
+
+  // Rotate to arbitrary basis
+  rotate_from_nth_canonical(x, mean);
+}
+
+void subsampling_random_vector (const gsl_rng* r, const gsl_vector* mean, double theta_max, gsl_vector* x)
+{
+  hollow_cone_random_vector(r, mean, 0, theta_max, x);
+}
+
+static double beta_inc_unnormalized (double a, double b, double x)
+{
+  return gsl_sf_beta_inc(a, b, x) * gsl_sf_beta(a, b);
+}
+
+static double incomplete_wallis_integral (double theta, unsigned int m)
+{
+  /**
+     @param theta 0 < theta < pi
+     @param m
+     @return \int_0^\theta \sin^m x dx
+   **/
+  if ((theta < 0) || (theta > M_PI))
+    GSL_ERROR("Incomplete Wallis integral only allows theta in [0,pi]", GSL_EDOM);
+  if (theta < M_PI_2)
+    return 0.5 * beta_inc_unnormalized((m+1)/2.0, 0.5, gsl_pow_2(sin(theta)));
+  else
+    return 0.5 * (gsl_sf_beta((m+1)/2.0, 0.5) + beta_inc_unnormalized(0.5, (m+1)/2.0, gsl_pow_2(cos(theta))));
+}
+
+double planar_angle_to_solid_angle (double planar_angle, unsigned int dimension)
+{
+  return 2 * pow(M_PI, (dimension - 1)/2.0)
+    * incomplete_wallis_integral(planar_angle, dimension - 2)
+    / gsl_sf_gamma((dimension - 1)/2.0);
+}
+
+double solid_angle_to_planar_angle (double solid_angle, unsigned int dimension)
+{
+  double f (double planar_angle, void* params)
+  {
+    return planar_angle_to_solid_angle(planar_angle, dimension) - solid_angle;
+  }
+
+  gsl_function gsl_f = {&f};
+  /* if (fabs(GSL_FN_EVAL(&gsl_f, 0))/surface_area_of_ball(dimension) < BISECTION_FUNCTION_EPSABS) */
+  /*   return 0; */
+  /* else if (fabs(GSL_FN_EVAL(&gsl_f, M_PI))/surface_area_of_ball(dimension) < BISECTION_FUNCTION_EPSABS) */
+  /*   return M_PI; */
+  /* else return bisection(&gsl_f, 0, M_PI); */
+  if (solid_angle == 0)
+    return 0;
+  else if (solid_angle == surface_area_of_ball(dimension))
+    return M_PI;
+  else return bisection(&gsl_f, 0, M_PI);
+}