Benchmark some geometry

benches/geometry.rs

@@ -0,0 +1,99 @@
+use criterion::{black_box, criterion_group, criterion_main, Criterion};
+use fast_fp::FF32;
+use nalgebra_v029::{self as na, center, distance_squared, Point3, Vector3};
+
+fn los_distance(c: &mut Criterion) {
+    let mut group = c.benchmark_group("los_distance");
+
+    // FF32 measures about 10% faster than f32 when tested on AMD 5950x with -Ctarget-cpu=native
+    // los_distance/f32        time:   [14.967 ns 14.983 ns 15.001 ns]
+    // los_distance/FF32       time:   [13.446 ns 13.451 ns 13.456 ns]
+
+    group.bench_function("f32", |b| {
+        let center_of_projection_a = Point3::new(4., 1., 0.).map(From::from);
+        let unit_line_of_sight_a = Vector3::new(1., 5., 0.).normalize().map(From::from);
+        let center_of_projection_b = Point3::new(3., 2., 0.).map(From::from);
+        let unit_line_of_sight_b = Vector3::new(-8., 1., 0.).normalize().map(From::from);
+
+        let sigma_o_squared = na::convert(6.84911050897861e-05_f64);
+        let sigma_p_squared = na::convert(0.000625_f64);
+
+        b.iter(|| {
+            calc_los_distance::<f32>(
+                black_box(center_of_projection_a),
+                black_box(unit_line_of_sight_a),
+                black_box(center_of_projection_b),
+                black_box(unit_line_of_sight_b),
+                black_box(sigma_o_squared),
+                black_box(sigma_p_squared),
+            )
+        })
+    });
+
+    group.bench_function("FF32", |b| {
+        let center_of_projection_a = Point3::new(4., 1., 0.).map(From::from);
+        let unit_line_of_sight_a = Vector3::new(1., 5., 0.).normalize().map(From::from);
+        let center_of_projection_b = Point3::new(3., 2., 0.).map(From::from);
+        let unit_line_of_sight_b = Vector3::new(-8., 1., 0.).normalize().map(From::from);
+
+        let sigma_o_squared = na::convert(6.84911050897861e-05_f64);
+        let sigma_p_squared = na::convert(0.000625_f64);
+
+        b.iter(|| {
+            calc_los_distance::<FF32>(
+                black_box(center_of_projection_a),
+                black_box(unit_line_of_sight_a),
+                black_box(center_of_projection_b),
+                black_box(unit_line_of_sight_b),
+                black_box(sigma_o_squared),
+                black_box(sigma_p_squared),
+            )
+        })
+    });
+}
+
+/// Given two line-of-sight rays, find the Mahalanobis distance between their nearest two points
+/// (given some distribution parameters)
+#[allow(non_snake_case)]
+fn calc_los_distance<T>(
+    center_of_projection_a: Point3<T>,
+    unit_line_of_sight_a: Vector3<T>,
+    center_of_projection_b: Point3<T>,
+    unit_line_of_sight_b: Vector3<T>,
+    sigma_o_squared: T,
+    sigma_p_squared: T,
+) -> T
+where
+    T: na::RealField + Copy,
+{
+    let A = center_of_projection_a;
+    let B = center_of_projection_b;
+    let a = unit_line_of_sight_a;
+    let b = unit_line_of_sight_b;
+    let c = B - A;
+
+    let ab = a.dot(&b);
+    let bc = b.dot(&c);
+    let ac = a.dot(&c);
+    let bb = b.dot(&b);
+    let aa = a.dot(&a);
+    let denom = aa * bb - ab * ab;
+    let D = A + a * ((-ab * bc + ac * bb) / denom);
+    let E = B + b * ((ab * ac - bc * aa) / denom);
+
+    let dist_squared = distance_squared(&D, &E);
+    let pos = center(&D, &E);
+
+    let r_a_squared = distance_squared(&pos, &A);
+    let r_b_squared = distance_squared(&pos, &B);
+
+    let sigma_a_squared = r_a_squared * sigma_o_squared + sigma_p_squared;
+    let sigma_b_squared = r_b_squared * sigma_o_squared + sigma_p_squared;
+
+    let mahalanobis_distance = (dist_squared / (sigma_a_squared + sigma_b_squared)).sqrt();
+
+    mahalanobis_distance
+}
+
+criterion_group!(benches, los_distance);
+criterion_main!(benches);

benches/operations.rs

@@ -3,12 +3,12 @@ use criterion::{
     BenchmarkId, Criterion, Throughput,
 };
 use fast_fp::{ff32, ff64, FF32, FF64};
-use ops::{Add, Div, Mul};
 use rand::{
     distributions::{self, Distribution},
     rngs::StdRng,
     Rng, SeedableRng,
 };
+use std::ops::{Add, Div, Mul};
 
 fn add(c: &mut Criterion) {
     let mut group = c.benchmark_group("add");