100 lines
3.6 KiB
Rust
100 lines
3.6 KiB
Rust
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);
|