use glam::DVec3;
#[derive(Clone, Copy, Debug, Default)]
pub struct Quadric {
a: [f64; 12],
pub w: f64,
}
impl Quadric {
pub fn plane(n: DVec3, d: f64, weight: f64) -> Quadric {
let (a, b, c) = (n.x, n.y, n.z);
Quadric {
a: [
a * a,
a * b,
a * c,
a * d,
b * b,
b * c,
b * d,
c * c,
c * d,
d * d,
0.0,
0.0,
]
.map(|v| v * weight),
w: weight,
}
}
pub fn constraint(n: DVec3, d: f64, weight: f64) -> Quadric {
Quadric {
w: 0.0,
..Quadric::plane(n, d, weight)
}
}
pub fn add(&mut self, o: &Quadric) {
for i in 0..12 {
self.a[i] += o.a[i];
}
self.w += o.w;
}
pub fn eval(&self, p: DVec3) -> f64 {
let a = &self.a;
let (x, y, z) = (p.x, p.y, p.z);
let v = a[0] * x * x
+ 2.0 * a[1] * x * y
+ 2.0 * a[2] * x * z
+ 2.0 * a[3] * x
+ a[4] * y * y
+ 2.0 * a[5] * y * z
+ 2.0 * a[6] * y
+ a[7] * z * z
+ 2.0 * a[8] * z
+ a[9];
v.max(0.0)
}
pub fn coeffs(&self) -> [f64; 12] {
self.a
}
#[inline(always)]
pub(crate) fn eval_m(&self, m: &[f64; 12]) -> f64 {
crate::reduce::kernels::dot12(&self.a, m).max(0.0)
}
pub fn mean_error(&self, p: DVec3) -> f64 {
if self.w <= 0.0 {
0.0
} else {
self.eval(p) / self.w
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn plane_distance() {
let q = Quadric::plane(DVec3::Y, -1.0, 2.0); assert!((q.eval(DVec3::new(5.0, 3.0, 0.0)) - 8.0).abs() < 1e-9);
assert!((q.mean_error(DVec3::new(0.0, 3.0, 0.0)) - 4.0).abs() < 1e-9);
}
}