70 lines
1.7 KiB
Rust
70 lines
1.7 KiB
Rust
#[inline(always)]
|
|
fn veclen(v: &[f32; 3]) -> f32 {
|
|
(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
|
|
}
|
|
|
|
#[inline(always)]
|
|
fn dist2(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
|
|
let dx = p1[0] - p2[0];
|
|
let dy = p1[1] - p2[1];
|
|
let dz = p1[2] - p2[2];
|
|
|
|
dx * dx + dy * dy + dz * dz
|
|
}
|
|
|
|
#[inline(always)]
|
|
fn dist(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
|
|
dist2(p1, p2).sqrt()
|
|
}
|
|
|
|
#[inline(always)]
|
|
fn distm(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
|
|
let dx = (p1[0] - p2[0]).abs();
|
|
let dy = (p1[1] - p2[1]).abs();
|
|
let dz = (p1[2] - p2[2]).abs();
|
|
dx + dy + dz
|
|
}
|
|
|
|
/// Dot product (cosine of angle) between two 3D vectors
|
|
#[inline(always)]
|
|
pub fn ndot_vec_dist(u: &[f32; 3], v: &[f32; 3]) -> f32 {
|
|
let z: [f32; 3] = [0.0; 3];
|
|
let lm = dist(u, &z) * dist(v, &z);
|
|
((u[0] * v[0]) + (u[1] * v[1]) + (u[2] * v[2])) / lm
|
|
}
|
|
|
|
/// Dot product (cosine of angle) between two 3D vectors
|
|
#[inline(always)]
|
|
pub fn ndot_vec_len(u: &[f32; 3], v: &[f32; 3]) -> f32 {
|
|
let lm = veclen(u) * veclen(v);
|
|
((u[0] * v[0]) + (u[1] * v[1]) + (u[2] * v[2])) / lm
|
|
}
|
|
|
|
#[inline(always)]
|
|
pub fn ndot_iter(u: &[f32; 3], v: &[f32; 3]) -> f32 {
|
|
let l_1: f32 = u.iter().map(|e| e * e).sum();
|
|
let l_2: f32 = v.iter().map(|e| e * e).sum();
|
|
let lm = (l_1 * l_2).sqrt();
|
|
let mut ret = 0.0;
|
|
for (a, b) in u.iter().zip(v.iter()) {
|
|
ret += a * b;
|
|
}
|
|
ret / lm
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod dot_impl_tests {
|
|
#[test]
|
|
fn test_dot_impls() {
|
|
use super::*;
|
|
let v1 = [1.0, 2.0, 3.0];
|
|
let v2 = [4.0, 5.0, 6.0];
|
|
let d1 = ndot_vec_dist(&v1, &v2);
|
|
let d2 = ndot_vec_len(&v1, &v2);
|
|
let d3 = ndot_iter(&v1, &v2);
|
|
assert!((d1 - d2) < 0.01);
|
|
assert!((d2 - d3) < 0.01);
|
|
assert!((d3 - d1) < 0.01);
|
|
}
|
|
}
|