use core::f32;
mod vec3;
pub use self::vec3::*;
mod vec4;
pub use self::vec4::*;
pub struct Sym3x3 {
x: [f32; 6],
}
impl Sym3x3 {
pub fn new(s: f32) -> Self {
Self {
x: [s, s, s, s, s, s],
}
}
pub fn weighted_covariance(points: &[Vec3], weights: &[f32]) -> Self {
assert!(points.len() == weights.len());
let total: f32 = weights.iter().sum();
let centroid: Vec3 = points.iter().zip(weights).map(|(p, &w)| p * w).sum();
let centroid = if total > f32::EPSILON {
centroid / total
} else {
centroid
};
let mut covariance = Sym3x3::new(0.0);
for (p, &w) in points.iter().zip(weights) {
let a: Vec3 = p - ¢roid;
let b = a * w;
covariance.x[..][0] += a.x() * b.x();
covariance.x[..][1] += a.x() * b.y();
covariance.x[..][2] += a.x() * b.z();
covariance.x[..][3] += a.y() * b.y();
covariance.x[..][4] += a.y() * b.z();
covariance.x[..][5] += a.z() * b.z();
}
covariance
}
pub fn principle_component(&self) -> Vec3 {
const POWER_ITERATION_COUNT: usize = 8;
let row0 = Vec4::new(self.x[0], self.x[1], self.x[2], 0.0);
let row1 = Vec4::new(self.x[0], self.x[1], self.x[2], 0.0);
let row2 = Vec4::new(self.x[0], self.x[1], self.x[2], 0.0);
let mut v = Vec4::new(1.0, 1.0, 1.0, 1.0);
for _ in 0..POWER_ITERATION_COUNT {
let w = row0 * v.splat_x();
let w = row1 * v.splat_y() + w;
let w = row2 * v.splat_z() + w;
let a = w.x().max(w.y().max(w.z()));
let a = Vec4::new(a, a, a, a);
v = w * a.reciprocal();
}
v.to_vec3()
}
}